//----------------------------------------------------------------------------
//  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 "CDeUtility.h"
#include "CDeConeFrustumTria.h"
#include "CDeConeTria.h"
#include "CDeCylTria.h"
#include "CDeConvexTorusTria.h"
#include "IntersLineSurfStd.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'origine nel centro della base minore e l'asse
// di simmetria del cono, rivolto verso la direzione di apertura, come asse Z.
bool
CDeSimpleConeFrustumTria( const Frame3d& frCone, double dMinRad, double dMaxRad, double dHeight, const Triangle3d& trTria)
{
  // Porto il triangolo nel sistema di riferimento del tronco di cono
   Triangle3d trMyTria = trTria ;
   trMyTria.ToLoc( frCone) ;

  // 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 = SegmentConeFrustum( 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 ;
      return false ;
   }
   else if ( nMinBaseTriaIndex == IPPT_YES) {
      double dU1, dU2 ;
      if ( LineDisc( ptLine, vtLine, ORIG, -Z_AX, dMinRad, dU1, dU2) == D_INFINITE_INT_LINE_ON_PLANE) {
         Point3d ptInt, ptInt2 ;
         int nLineTriaIndex = IntersLineTria( ptLine + dU1 * vtLine, ptLine + dU2 * vtLine, trMyTria, ptInt, ptInt2) ;
         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 ;
      return false ;
   }
   else if ( nMaxBaseTriaIndex == IPPT_YES) {
      double dU1, dU2 ;
      if ( LineDisc( ptLine, vtLine, Point3d( 0., 0., dHeight), Z_AX, dMaxRad, dU1, dU2) == D_INFINITE_INT_LINE_ON_PLANE) {
         Point3d ptInt, ptInt2 ;
         int nLineTriaIndex = IntersLineTria( ptLine + dU1 * vtLine, ptLine + dU2 * vtLine, trMyTria, ptInt, ptInt2) ;
         if ( nLineTriaIndex != ILTT_NO  &&  nLineTriaIndex != ILTT_IN)
            return true ;
      }
   }
  // Triangolo tangente al tronco di cono
   Vector3d vtTriaNormXY( vtN.x, vtN.y, 0.) ;
   if ( ! vtTriaNormXY.Normalize())
      return false ;
   Point3d ptContactSt( dMinRad * vtTriaNormXY.x, dMinRad * vtTriaNormXY.y, 0.) ;
   Point3d ptContactEn( dMaxRad * vtTriaNormXY.x, dMaxRad * vtTriaNormXY.y, dHeight) ;
   Point3d ptInt, ptInt2 ;
   int nContactIndex = IntersLineTria( ptContactSt, ptContactEn, trMyTria, ptInt, ptInt2) ;
   return ! ( nContactIndex == ILTT_NO  ||  nContactIndex == ILTT_IN) ;
}

//----------------------------------------------------------------------------
// 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
CDeConeFrustumTria( const Frame3d& frCone, double dBaseRad, double dTopRad, double dHeight,
                    const Triangle3d& trTria, double dSafeDist)
{
  // Se il tronco di cono o il triangolo non sono ben definiti non procedo
   if ( max( dBaseRad, dTopRad) < EPS_SMALL  ||  dHeight < EPS_SMALL  ||  ! trTria.IsValid())
      return false ;

  // Se è un cilindro chiamo la routine apposita.
   if ( abs( dBaseRad - dTopRad) < EPS_SMALL)
      return CDeCylTria( frCone, max( dBaseRad, dTopRad), dHeight, trTria, dSafeDist) ;

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

  // Se è un cono, chiamo la routine apposita
   if ( dMinRad < EPS_SMALL)
      return CDeConeTria( frMyCone, dMaxRad, dHeight, trTria, dSafeDist) ;

  // Se distanza di sicurezza nulla
   if ( dSafeDist < EPS_SMALL)
      return CDeSimpleConeFrustumTria( frMyCone, dBaseRad, dTopRad, dHeight, trTria) ;

  // Verifica preliminare con tronco di cono esteso
   double dDiffRad = dMaxRad - dMinRad ;
   double dDeltaVert = dSafeDist * sqrt( 1 + ( dHeight * dHeight / ( dDiffRad * dDiffRad))) ;
   double dDeltaMaxRad = ( dDeltaVert + dSafeDist) * dDiffRad / dHeight ;
   double dDeltaMinRad = ( dDeltaVert - dSafeDist) * dDiffRad / dHeight ;
   Frame3d frTmp = frMyCone ; frTmp.Translate( - dSafeDist * frMyCone.VersZ()) ;
   if ( ! CDeSimpleConeFrustumTria( frTmp, dMinRad + dDeltaMinRad, dMaxRad + dDeltaMaxRad, dHeight + 2 * dSafeDist, trTria))
      return false ;

  // Tronco di cono intermedio
   double dHypo = sqrt( dDiffRad * dDiffRad + dHeight * dHeight ) ;
   double dDeltaH = dSafeDist * dDiffRad / dHypo ;
   double dDeltaR = dSafeDist * dHeight / dHypo ;
   frTmp = frMyCone ; frTmp.Translate( -dDeltaH * frMyCone.VersZ()) ;
   if ( CDeSimpleConeFrustumTria( frTmp, dMinRad + dDeltaR, dMaxRad + dDeltaR, dHeight, trTria))
      return true ;

  // Toro in basso
   if ( CDeSimpleConvexTorusTria( frMyCone, dMinRad, dSafeDist, CT_TOT, trTria))
      return true ;

  // Toro in alto
   frTmp = frMyCone ; frTmp.Translate( dHeight * frMyCone.VersZ()) ;
   if ( CDeSimpleConvexTorusTria( frTmp, dMaxRad, dSafeDist, CT_TOT, trTria))
      return true ;

   return false ;
}