#include "stdafx.h"
#include "CDeUtility.h"
#include "/EgtDev/Include/EGkDistPointLine.h"
#include "/EgtDev/Include/ENkPolynomialRoots.h"

using namespace std ;

//----------------------------------------------------------------------------
// Dati un punto e una retta, restituisce il quadrato della loro distanza.
// La retta è descritta da un suo punto e dal suo versore tangente.
double
GetPointLineSqDist( const Point3d& ptP, const Point3d& ptLine, const Vector3d& vtLine)
{
   return ( ( ptP - ptLine) - ( ptP - ptLine) * vtLine * vtLine).SqLen() ;
}

//----------------------------------------------------------------------------
// Dati un punto e un piano, ne restituisce la distanza con segno.
// Il piano è descritto da un suo punto e dal suo versore normale.
double
PointPlaneSignedDist( const Point3d& ptP, const Point3d& ptPlane, const Vector3d& vtNorm)
{
   return ( ptP - ptPlane) * vtNorm ;
}

//----------------------------------------------------------------------------
// Dati un sistema di tre punti e un piano, ne calcola le tre distanze con segno e ne restituisce la
// maggiore. I tre punti sono passati mediante un triangolo, il piano è descritto da un suo punto e
// dal suo versore normale.
double
ThreePointPlaneSignedDist( const Triangle3d& trTria, const Point3d& ptPlane, const Vector3d& vtNorm)
{
   double dMaxSgnDist = - INFINITO ;
   for ( int nV = 0 ; nV < 3 ; ++ nV) {
      double dCurrSgnDist = PointPlaneSignedDist( trTria.GetP( nV), ptPlane, vtNorm) ;
      if ( dCurrSgnDist > dMaxSgnDist)
         dMaxSgnDist = dCurrSgnDist ;
   }
   return dMaxSgnDist ;
}

//----------------------------------------------------------------------------
// Calcola, se esiste, il punto di una retta a distanza con segno data da un piano.
// Il piano è descritto da punto e versore normale.
// La retta è descritta da punto e versore direzione.
// I casi possibili sono:
// - Nessun punto della retta dista d dal piano (retta parallela al piano con distanza diversa da d),
//   viene restituito 0 e il valore di dPar non ha senso.
// - Un solo punto della retta dista d dal piano (retta non parallela al piano), viene restituito 1
//   e il valore di dPar è il parametro del punto sulla retta.
// - Tutti i punti della retta distano d dal piano (retta parallela al piano con distanza d), viene 
//   restituito 2 e qualunque valore di dPar ha senso. 
int
LinePlaneDDistPar( const Point3d& ptPlane, const Vector3d& vtPlane, const Point3d& ptLine, const Vector3d& vtLine,
                   double dDist, double& dPar)
{
   // Vettore congiungente il punto del piano con il punto iniziale della retta
   Vector3d vtPlLn = ptLine - ptPlane ;
   // Distanza con segno del punto iniziale della retta dal piano
   double dSgnDist0 = vtPlLn * vtPlane ;
   // Prodotto scalare fra versore della direzione della retta e versore normale al piano
   double dDotDirNorm = vtLine * vtPlane ;
   // Retta parallela al piano
   if ( abs( dDotDirNorm) < EPS_ZERO) {
      if ( abs( dSgnDist0 - dDist) < EPS_SMALL)
         return 2 ;
      else
         return 0 ;
   }
   // Caso generale
   else {
      dPar = ( dDist - dSgnDist0) / dDotDirNorm ;
      return 1 ;
   }
}

//----------------------------------------------------------------------------
// Date due rette nello spazio restituisce il quadrato della loro distanza.
// Le rette sono definite da un punto e dal versore direzione.
double
LineLineSqDist( const Point3d& ptP1, const Vector3d& vtD1, const Point3d& ptP2, const Vector3d& vtD2)
{
   // Se parallele
   if ( abs( 1. - abs( vtD1 * vtD2)) < EPS_SMALL)
      return ( ( ptP2 - ptP1) - ( ptP2 - ptP1) * vtD1 * vtD1).SqLen() ;
   // Caso generale
   else {
      Vector3d vtOrtD2 = vtD2 - vtD2 * vtD1 * vtD1 ;
      Vector3d vtOrtP1P2 = ( ptP2 - ptP1) - (ptP2 - ptP1) * vtD1 * vtD1 ;
      double dD = ( vtOrtD2 * vtOrtP1P2) / vtOrtP1P2.SqLen() ;
      return ( vtOrtP1P2.SqLen() + dD * dD * vtOrtD2.SqLen() + 2 * dD * vtOrtD2 * vtOrtP1P2) ;
   }
}

//----------------------------------------------------------------------------
// Data una retta e un segmento, ne restituisce il quadrato della distanza.
// La retta è definita da un punto e dal versore direzione.
// Il segmento è definito da un estremo, dal versore direzione verso l'altro estremo e dalla lunghezza.
double
LineSegmentSqDist( const Point3d& ptPLn, const Vector3d& vtDLn,
                   const Point3d& ptPSg, const Vector3d& vtDSg, double dSgLen)
{
   // Se paralleli
   if ( AreSameOrOppositeVectorApprox( vtDLn, vtDSg))
      return ( ( ptPSg - ptPLn) - ( ptPSg - ptPLn) * vtDLn * vtDLn).SqLen() ;

   // Caso generale
   else {
      double dDLn = ( ptPSg - ptPLn) * vtDLn ;
      double dDSg = ( ptPSg - ptPLn) * vtDSg ;
      double dDLnSg = vtDLn * vtDSg ;
      // posizione parametrica del punto sul segmento
      double dSgPar = ( dDLn * dDLnSg - dDSg) / ( 1. - dDLnSg * dDLnSg) ;
      // se prima dell'inizio, è distanza dell'inizio dalla linea
      if ( dSgPar <= 0.)
         return GetPointLineSqDist( ptPSg, ptPLn, vtDLn) ;
      // se dopo fine, è distanza fine dalla linea
      else if ( dSgPar >= dSgLen)
         return GetPointLineSqDist( ptPSg + dSgLen * vtDSg, ptPLn, vtDLn) ;
      // altrimenti, determino i due punti a minima distanza
      else {
         double dLnPar = ( dDLn - dDSg * dDLnSg) / ( 1. - dDLnSg * dDLnSg) ;
         Point3d ptMinLn = ptPLn + dLnPar * vtDLn ;
         Point3d ptMinSg = ptPSg + dSgPar * vtDSg ;
         return SqDist( ptMinLn, ptMinSg) ;
      }
   }
}

//----------------------------------------------------------------------------
// Valuta l'interferenza di una circonferenza (disco vuoto) e un triangolo (pieno) complanari.
// Si assume che disco e piano siano paralleli, ovvero che valga a n = 1 con
// a versore dell'asse del disco e n versore normale al triangolo.
// L'informazione su n è contenuta nell'oggetto triangolo; quella su a non è passata
// in quanto inutile sotto le nostre assunzioni.
bool
CoplanarCircleTriangleInterference( const Point3d& ptCen, double dRad, const Triangle3d& trTria)
{
   // Se il centro del disco non sta sul piano del triangolo, non sono complanari
   if ( abs( PointPlaneSignedDist( ptCen, trTria.GetP( 0), trTria.GetN())) > EPS_SMALL)
      return false ;
   // Classifico i vertici del triangolo in base alla loro posizione rispetto alla circonferenza.
   double dSqRad = dRad * dRad ;
   double dSqRadInf = dSqRad - 2 * dRad * EPS_SMALL ;
   double dSqRadSup = dSqRad + 2 * dRad * EPS_SMALL ;
   int nInOutVec[3] ;
   for ( int n = 0 ; n < 3 ; ++ n) {
      Point3d ptTriaVert = trTria.GetP( n) ;
      nInOutVec[n] = SqDist( ptTriaVert, ptCen) < dSqRadInf ? 1 : SqDist( ptTriaVert, ptCen) < dSqRadSup ? 0 : - 1 ;
   }
   // Se i vertici sono tutti dentro, non c'è interferenza.
   if ( nInOutVec[0] == 1  &&  nInOutVec[1] == 1  &&  nInOutVec[2] == 1)
      return false ;
   // Se i vertici sono tutti fuori, valuto se il centro della circonferenza è interno al triangolo.
   else if ( nInOutVec[0] == - 1  &&  nInOutVec[1] == - 1  &&  nInOutVec[2] == - 1) {
      return IsPointInsideTriangle( ptCen, trTria, TriangleType::EXACT) ;
   }
   // Almeno un vertice tocca la circonferenza oppure due segmenti la attraversano, quindi c'è interferenza.
   else
      return true ;
}

//----------------------------------------------------------------------------
// Valuta l'interferenza di un disco e un triangolo complanari.
// Si assume che disco e piano siano paralleli, ovvero che valga a n = 1 con
// a versore dell'asse del disco e n versore normale al triangolo.
// L'informazione su n è contenuta nell'oggetto triangolo; quella su a non è passata
// in quanto inutile sotto le nostre assunzioni.
bool
CoplanarDiscTriangleInterference( const Point3d& ptCen, double dRad, const Triangle3d& trTria, int nTriType)
{
   // Se il centro del disco non sta sul piano del triangolo, non sono complanari
   if ( abs( PointPlaneSignedDist( ptCen, trTria.GetP( 0), trTria.GetN())) > EPS_SMALL)
      return false ;
   // Se il centro giace nel triangolo
   if ( IsPointInsideTriangle( ptCen, trTria, TriangleType::EXACT))
      return true ;

   double dSqMinDist = 0 ;
   switch ( nTriType) {
   case OPEN:
      dSqMinDist = dRad * dRad - 2 * dRad * EPS_SMALL ;
      break ;
   case EXACT:
      dSqMinDist = dRad * dRad ;
      break ;
   case CLOSED:
      dSqMinDist = dRad * dRad + 2 * dRad * EPS_SMALL ;
      break ;
   }

   // Se il centro non si avvicina più del raggio dalle rette associate ai segmenti, non c'è interferenza
   double dSegPointDist0, dSegPointDist1, dSegPointDist2 ;
   ( DistPointLine( ptCen, trTria.GetP( 0), trTria.GetP( 1))).GetSqDist( dSegPointDist0) ;
   ( DistPointLine( ptCen, trTria.GetP( 1), trTria.GetP( 2))).GetSqDist( dSegPointDist1) ;
   ( DistPointLine( ptCen, trTria.GetP( 2), trTria.GetP( 0))).GetSqDist( dSegPointDist2) ;
   if ( dSegPointDist0 > dSqMinDist  &&  dSegPointDist1 > dSqMinDist  &&  dSegPointDist2 > dSqMinDist)
      return false ;
   // interferenza
   return true ;
}

//----------------------------------------------------------------------------
// Date due rette, descritte da un punto e un vettore direzione,
// trova i parametri corrispondenti ai punti di minima distanza. 
// Se le rette non sono parallele abbiamo una singola coppia di
// punti e viene restituito true, altrimenti ne abbiamo infinite
// e viene restituito false. Si noti che i parametri dU1, dU2   
// non hanno necessariamente il significato di lunghezza;
// perché non è richiesto che i vettori siano normalizzati.
bool
FindLineLineMinDistPar( const Point3d& ptL1, const Vector3d& vtV1,
                        const Point3d& ptL2, const Vector3d& vtV2,
                        double& dU1, double& dU2)
{
   // Se le rette sono parallele
   if ( AreSameOrOppositeVectorExact( vtV1, vtV2))
      return false ;

   // Vettore congiungente i punti iniziali
   Vector3d vtD = ptL2 - ptL1 ;
   // Proiezioni di vtD sui versori direzione
   double dD1 = vtD * vtV1 ;
   double dD2 = vtD * vtV2 ;
   // Prodotto scalare fra i versori direzione
   double dV = vtV1 * vtV2 ;

   // Se le rette sono perpendicolari
   if ( abs( vtV1 * vtV2) < EPS_ZERO) {
      dU1 = dD1 ;
      dU2 = - dD2 ;
   }
   // Caso generale
   else {
      dU1 = ( dD1 - dD2 * dV) / ( 1 - dV * dV) ;
      dU2 = ( - dD2 + dD1 * dV) / ( 1 - dV * dV) ;
   }
   return true ;
}

//----------------------------------------------------------------------------
// Calcolo delle posizioni di tangenza di una sfera che si muove dalla posizione
// originale lungo una direzione prefissata rispetto ad una retta.
// Le posizioni sono rappresentate dalla distanza percorsa.
// Il parametro di ritorno può assumere i seguenti valori :
//  0 : la sfera non è mai tangente alla retta; dU1 e dU2 non hanno senso
//  1 : la sfera è tangente alla retta in una posizione; solo dU1 ha senso 
//  2 : la sfera è tangente alla retta in due posizioni; dU1 e dU2 valide
//  3 : la sfera è sempre tangente 
int
SphereLineTangentPoints( const Point3d& ptSpheCen, double dSpheRad,
                         const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen,
                         const Vector3d& vtMove, double& dU1, double& dU2)
{
   // Definisco l'equazione
   Vector3d vtMotOrt = vtMove - vtMove * vtSegDir * vtSegDir ;
   Vector3d vtLineSeg = ptSeg - ptSpheCen ;
   Vector3d vtLineSegOrt = vtLineSeg - vtLineSeg * vtSegDir * vtSegDir ;
   // Setto i coefficienti dell'equazione
   DBLVECTOR vdCoeff( 3) ;
   vdCoeff[0] = vtLineSegOrt.SqLen() - dSpheRad * dSpheRad ;
   vdCoeff[1] = - 2 * vtMotOrt * vtLineSegOrt ;
   vdCoeff[2] = vtMotOrt.SqLen() ;
   if ( vdCoeff[2] < EPS_ZERO * EPS_ZERO) {
      if ( abs( vdCoeff[0]) < EPS_SMALL)
         return 3 ;
      else
         return 0 ;
   }
   // Risolvo l'equazione
   DBLVECTOR vdRoots ;
   int nRoots = PolynomialRoots( 2, vdCoeff, vdRoots) ;
   // Studio le soluzioni
   if ( nRoots == 1)
      dU1 = vdRoots[0] ;
   else if ( nRoots == 2) {
      dU1 = min( vdRoots[0], vdRoots[1]) ;
      dU2 = max( vdRoots[0], vdRoots[1]) ;
   }
   return nRoots ;
}

//----------------------------------------------------------------------------
// Valuta l'allontanamento di un disco, lungo il proprio asse di simmetria, da un piano.
// Il disco è descritto dal centro nella posizione iniziale e dal raggio (la normale è il versore traslazione).
// Il piano è descritto da un suo punto e dal suo versore normale.
// Il moto è descritto dal versore di traslazione.
// Il valore restituito è un intero che descrive la situazione di allontanamento e,
// quando sensato, per referenza restituisce il punto di ultimo contatto.
// 0: Nessun contatto
// 1: Un contatto
// 2: Disco e piano ortogonali, con disco tg al piano, il contatto è una semiretta
// 3: Disco e piano ortogonali, con disco secante il piano, il contatto è una striscia 
// 4: Disco e piano paralleli, come ultimo contatto si restituisce il centro.
// Nel caso 0 il parametro ptTouch non ha senso, così come nel caso 2 e nel caso 3
int
DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad,
                             const Point3d& ptPlane, const Vector3d& vtPlane,
                             const Vector3d& vtMove, Point3d& ptTouch)
{
   // Se disco e piano sono paralleli
   if ( AreSameOrOppositeVectorApprox( vtMove, vtPlane)) {
      double dDist = PointPlaneSignedDist( ptPlane, ptDiskCen, vtMove) ;
      if ( dDist > - EPS_SMALL) {
         ptTouch = ptDiskCen + dDist * vtMove ;
         return 4 ;
      }
      return 0 ;
   }
   // Vettore radiale
   Vector3d vtRadLine = vtMove * ( vtPlane * vtMove) - vtPlane ;
   // Disco e piano ortogonali
   if ( vtRadLine.IsNormalized()) {
      double dDist = abs( PointPlaneSignedDist( ptPlane, ptDiskCen, vtMove)) ;
      if ( abs( dDist - dDiskRad) < EPS_SMALL)
         return 2 ;
      else if ( dDist < dDiskRad)
         return 3 ;
      return 0 ;
   }
   // Cerco un punto di contatto nell'interno del triangolo. Se tale punto
   // esiste, la retta intersezione fra il piano del triangolo e quello del
   // disco è tangente alla circonferenza.
   vtRadLine.Normalize() ;
   // Punti delle due rette candidate all'intersezione col triangolo
   Point3d ptStPlus = ptDiskCen + dDiskRad * vtRadLine ;
   Point3d ptStMinus = ptDiskCen - dDiskRad * vtRadLine ;
   // Parametri d'intersezione delle rette col piano
   double dDistPlus = ( ( ptPlane - ptStPlus) * vtPlane) / ( vtMove * vtPlane) ;
   double dDistMinus = ( ( ptPlane - ptStMinus) * vtPlane) / ( vtMove * vtPlane) ;
   double dDist ;
   if ( dDistPlus > dDistMinus) {
      dDist = dDistPlus ;
      ptTouch = ptStPlus + dDist * vtMove ;
   }
   else {
      dDist = dDistMinus ;
      ptTouch = ptStMinus + dDist * vtMove ;
   }
   if ( dDist > - EPS_SMALL)
      return 1 ;
   return 0 ;
}

//----------------------------------------------------------------------------
// Si limita il segmento alla parte di spazio con distanza positiva dal piano.
// Se segmento ancora valido si restituisce true, altrimenti false.
bool
ClampSegmentOutPlane( const Plane3d& plPlane, Point3d& ptSegP, const Vector3d& vtSegV, double& dSegLen, double dSignTol)
{
   // Distanze degli estremi del segmento dal piano
   double dDistStart = DistPointPlane( ptSegP, plPlane) ;
   double dDistEnd = DistPointPlane( ptSegP + dSegLen * vtSegV, plPlane) ;
   // Se il segmento sta nel piano o è tutto esterno, va bene
   if ( dDistStart > dSignTol - EPS_ZERO  &&  dDistEnd > dSignTol - EPS_ZERO)
      return true ;
   // Se il segmento è tutto interno, va eliminato
   if ( dDistStart < dSignTol  &&  dDistEnd < dSignTol) {
      dSegLen = 0 ;
      return false ;
   }
   // Altrimenti il segmento attraversa il piano e va limitato
   double dIntersLen = dSegLen * abs( dDistStart) / ( abs( dDistStart) + abs( dDistEnd)) ;
   if ( dDistStart > 0)
      dSegLen = dIntersLen ;
   else {
      ptSegP += dIntersLen * vtSegV ;
      dSegLen -= dIntersLen ;
   }
   return true ;
}