//----------------------------------------------------------------------------
//  EgalTech 2022-2022
//----------------------------------------------------------------------------
// File : CDeCapsTria.cpp   Data : 14.05.22     Versione : 2.4e2
// Contenuto : Implementazione della verifica di collisione tra 
//             Capsule (cilindro con estremità semisferiche) e Triangle3d.
//
//
// Modifiche :14.05.22 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "CDeCapsTria.h"
#include "CDeSpheTria.h"
#include "ProjPlane.h"
#include "IntersLineCaps.h"
#include "/EgtDev/Include/EGkPolygon3d.h"
#include "/EgtDev/Include/EGkIntersLinePlane.h"
#include "/EgtDev/Include/EGkDistPointTria.h"
#include "/EgtDev/Include/EGkIntersLineSphere.h"

using namespace std ;

//----------------------------------------------------------------------------
bool
CDeSimpleCapsTria( const Point3d& ptP1, const Point3d& ptP2, double dR, const Triangle3d& trTria)
{
   // vedi Ericson, Real-Time Collision Detection, pag. 226 (Nettle method)

  // Dati della capsule come sfera che si muove
   Point3d ptC = ptP1 ;
   Point3d ptE = ptP2 ;
   Vector3d vtDir = ptP2 - ptP1 ;
   double dLen = vtDir.Len() ;
   if ( dLen < EPS_SMALL)
      return CDeSimpleSpheTria( Media( ptP1, ptP2), dR, trTria) ;
   vtDir /= dLen ;
   if ( vtDir * trTria.GetN() > 0) {
      vtDir.Invert() ;
      ptC = ptP2 ;
      ptE = ptP1 ;
   }
  // Se sfera finale dista dal piano come o meno del raggio, devo verificarla direttamente (il retro è escluso dal calcolo standard)
   double dDistE = DistPointPlane( ptE, trTria.GetPlane()) ;
   if ( abs( dDistE) <= dR) {
      if ( CDeSimpleSpheTria( ptE, dR, trTria))
         return true ;
   }
  // Determinazione primo possibile punto di contatto della sfera con il piano
   Point3d ptD = ptC - trTria.GetN() * dR ;
  // Intersezione della linea di movimento di questo punto con il piano del triangolo
   Point3d ptP ;
   int nLpRes = IntersLinePlane( ptD, vtDir, 1, trTria.GetPlane(), ptP, false) ;
  // Se non c'è intersezione passante
   if ( nLpRes != ILPT_YES) {
     // se il centro dista dal piano non meno del raggio, allora non c'è sicuramente collisione
      double dDistM = DistPointPlane( Media( ptP1, ptP2), trTria.GetPlane()) ;
      if ( abs( dDistM) >= dR)
         return false ;
     // interseco l'asse del capsule con capsule di pari raggio con asse i lati del triangolo
      double dU1 = INFINITO, dU2 = -INFINITO ;
      for ( int i = 0 ; i < 3 ; ++ i) {
         double dInt1, dInt2 ;
         if ( IntersLineCaps( ptC, vtDir, trTria.GetP( i), trTria.GetP( ( i + 1) % 3), dR, dInt1, dInt2)) {
            dU1 = min( dU1, dInt1) ;
            dU2 = max( dU2, dInt2) ;
            if ( ! ( dU1 >= dLen  ||  dU2 <= 0))
               return true ;
         } 
      }
      return false ;
   }
  // Determino la posizione dell'intersezione rispetto al triangolo
   DistPointTriangle dptDist( ptP, trTria) ;
  // Se l'intersezione sta nel triangolo
   double dSqDist ;
   if ( dptDist.GetSqDist( dSqDist)  &&  dSqDist < 4 * SQ_EPS_SMALL) {
      double dPos = ( ptP - ptD) * vtDir ;
      return ( dPos > 0  &&  dPos < dLen) ;
   }
  // Altrimenti, recupero il punto del triangolo più vicino all'intersezione
   Point3d ptQ ;
   if ( dptDist.GetMinDistPoint( ptQ)) {
      Point3d ptI1, ptI2 ;
      int nLsRes = IntersLineSphere( ptQ, -vtDir, ptC, dR, ptI1, ptI2) ;
      if ( nLsRes != ILST_SEC)
         return false ;
      double dPos = ( ptQ - ptI1) * vtDir ;
      return ( dPos > 0  &&  dPos < dLen) ;
   }

   return false ;
}

//----------------------------------------------------------------------------
bool
CDeCapsTria( const Point3d& ptP1, const Point3d& ptP2, double dR, const Triangle3d& trTria, double dSafeDist)
{
   return CDeSimpleCapsTria( ptP1, ptP2, dR + max( 0., dSafeDist), trTria) ;
}