//----------------------------------------------------------------------------
//  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 "/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 ;
   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 ;
   }
  // 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 dDist = DistPointPlane( ptP, trTria.GetPlane()) ;
      if ( abs( dDist) >= dR)
         return false ;
     // !!! DA FARE !!!! 
     // si deve intersecare l'asse del capsule con i cilindri centrati sui lati del triangolo
     // se intersezione inferiore a dLen allora collisione
     // altrimenti si deve intersecare l'asse del capsule con le sfere centrate sui vertici del triangolo
     // se intersezione inferiore a dLen allora collisione
     // per ora salto
      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 > -dR  &&  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 > -dR  &&  dPos < dLen) ;
   }

   return false ;
}

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