//----------------------------------------------------------------------------
//  EgalTech 2020-2024
//----------------------------------------------------------------------------
// File : DistLineLine.cpp      Data : 10.05.24         Versione : 2.6e3
// Contenuto : Implementazione della classe distanza fra elementi lineari.
//
//
//
// Modifiche : 06.11.20 LM Creazione modulo.
//             12.08.22 DS Correzioni e migliorie varie.
//
//----------------------------------------------------------------------------

#include "stdafx.h"
#include "/EgtDev/Include/EGkDistLineLine.h"
#include "/EgtDev/Include/EgtNumUtils.h"
#include "/EgtDev/Include/EGkGeoCollection.h"
#include "/EgtDev/Include/EGkGeoConst.h"

using namespace std ;

//----------------------------------------------------------------------------
DistLineLine::DistLineLine( const Point3d& ptSt1, const Point3d& ptEn1,
                            const Point3d& ptSt2, const Point3d& ptEn2,
                            bool bIsSegment1, bool bIsSegment2)
{
  // reset oggetto
   m_dSqDist = - 1 ;
   m_dDist = - 1 ;
  // calcolo direzione segmenti
   Vector3d vtD1 = ptEn1 - ptSt1 ;
   double dLen1 = vtD1.Len() ;
   Vector3d vtD2 = ptEn2 - ptSt2 ;
   double dLen2 = vtD2.Len() ;
   if ( dLen1 < EPS_SMALL  ||  dLen2 < EPS_SMALL)
      return ;
   vtD1 /= dLen1 ;
   vtD2 /= dLen2 ;
  // eseguo
   Calculate( ptSt1, vtD1, dLen1, ptSt2, vtD2, dLen2, bIsSegment1, bIsSegment2) ;
}

//----------------------------------------------------------------------------
// I vettori devono essere normalizzati
DistLineLine::DistLineLine( const Point3d& ptSt1, const Vector3d& vtD1, double dLen1,
                            const Point3d& ptSt2, const Vector3d& vtD2, double dLen2,
                            bool bIsSegment1, bool bIsSegment2)
{
  // reset oggetto
   m_dSqDist = - 1 ;
   m_dDist = - 1 ;
  // verifico segmenti
   if ( dLen1 < EPS_SMALL  ||  dLen2 < EPS_SMALL)
      return ;
  // eseguo
   Calculate( ptSt1, vtD1, dLen1, ptSt2, vtD2, dLen2, bIsSegment1, bIsSegment2) ;
}

//----------------------------------------------------------------------------
bool
DistLineLine::GetSqDist( double& dSqDist) const
{
   if ( m_dSqDist < 0)
      return false ;

   dSqDist = m_dSqDist ;
   return true ;
}

//----------------------------------------------------------------------------
bool
DistLineLine::GetDist( double& dDist) const
{
   if ( m_dSqDist < 0)
      return false ;

   if ( m_dDist < 0)
      m_dDist = sqrt( m_dSqDist) ;
   dDist = m_dDist ;
   return true ;
}

//----------------------------------------------------------------------------
bool
DistLineLine::GetMinDistPoints( Point3d& ptMinDist1, Point3d& ptMinDist2) const
{
   if ( m_dSqDist < 0)
      return false ;
   ptMinDist1 = m_ptMinDist1 ;
   ptMinDist2 = m_ptMinDist2 ;
   return true ;
}

//----------------------------------------------------------------------------
bool
DistLineLine::GetPositionsAtMinDistPoints( double& dPos1, double& dPos2) const
{
   if ( m_dSqDist < 0)
      return false ;
   dPos1 = m_dPos1 ;
   dPos2 = m_dPos2 ;
   return true ;
}

//----------------------------------------------------------------------------
// Calcola la distanza fra i due elementi lineari, i punti di minima distanza e le loro posizioni.
// Se i due elementi sono paralleli i punti di minimo sono scelti secondo convenienza.
void
DistLineLine::Calculate( const Point3d& ptSt1, const Vector3d& vtD1, double dLen1,
                         const Point3d& ptSt2, const Vector3d& vtD2, double dLen2,
                         bool bIsSegment1, bool bIsSegment2)
{
  // Se elementi paralleli o antiparalleli
   if ( AreSameOrOppositeVectorExact( vtD1, vtD2)) {
     // Se il primo elemento è una retta infinita
      if ( ! bIsSegment1) {
         Vector3d vtStSt = ptSt2 - ptSt1 ;
         double dLong = vtStSt * vtD1 ;
         Vector3d vtDist = vtStSt - dLong * vtD1 ;
         m_dSqDist = vtDist.SqLen() ;
         m_dPos1 = dLong ;
         m_dPos2 = 0 ;
         m_ptMinDist1 = ptSt1 + dLong * vtD1 ;
         m_ptMinDist2 = ptSt2 ;
      } 
     // se altrimenti il secondo elemento è una retta infinita
      else if ( ! bIsSegment2) {
         Vector3d vtStSt = ptSt1 - ptSt2 ;
         double dLong = vtStSt * vtD2 ;
         Vector3d vtDist = vtStSt - dLong * vtD2 ;
         m_dSqDist = vtDist.SqLen() ;
         m_dPos1 = 0 ;
         m_dPos2 = dLong ;
         m_ptMinDist1 = ptSt1 ;
         m_ptMinDist2 = ptSt2 + dLong * vtD2 ;
      }
     // altrimenti entrambi gli elementi sono segmenti
      else {
         Point3d ptEn1 = ptSt1 + dLen1 * vtD1 ;
         Point3d ptEn2 = ptSt2 + dLen2 * vtD2 ;
         Vector3d vtStSt = ptSt2 - ptSt1 ;
         Vector3d vtStEn = ptEn2 - ptSt1 ;
         double dStU = vtStSt * vtD1 ;
         double dEnU = vtStEn * vtD1 ;
        // Classifico i punti del secondo segmento in base alla loro
        // coordinata rispetto all'ordinamento generato dal primo.
         double dMinPar, dMaxPar ;
         Point3d ptMinPar, ptMaxPar ;
         if ( dStU < dEnU) {
            ptMinPar = ptSt2 ;
            ptMaxPar = ptEn2 ;
            dMinPar = dStU ;
            dMaxPar = dEnU ;
         }
         else {
            ptMinPar = ptEn2 ;
            ptMaxPar = ptSt2 ;
            dMinPar = dEnU ;
            dMaxPar = dStU ;
         }
        // Possibili posizioni reciproche dei segmenti
         if ( dMinPar > dLen1) {
            m_dSqDist = SqDist( ptEn1, ptMinPar) ;
            m_ptMinDist1 = ptEn1 ;
            m_ptMinDist2 = ptMinPar ;
            m_dPos1 = dLen1 ;
            m_dPos2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
         }
         else if ( dMinPar > 0) {
            m_dSqDist = max( vtStSt * vtStSt - dStU * dStU, 0.) ;
            m_ptMinDist1 = ptSt1 + dMinPar * vtD1 ;
            m_ptMinDist2 = ptMinPar ;
            m_dPos1 = dMinPar ;
            m_dPos2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
         }
         else if ( dMaxPar > 0) {
            m_dSqDist = max( vtStSt * vtStSt - dStU * dStU, 0.) ;
            m_ptMinDist1 = ptSt1 ;
            m_ptMinDist2 = ptSt2 + ( ptSt1 - ptSt2) * vtD2 * vtD2 ;
            m_dPos1 = 0 ;
            m_dPos2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
         }
         else {
            m_dSqDist = SqDist( ptSt1, ptMaxPar) ;
            m_ptMinDist1 = ptSt1 ;
            m_ptMinDist2 = ptMaxPar ;
            m_dPos1 = 0 ;
            m_dPos2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
         }
      }
   }
  // Caso generale
   else {
     // Posizioni a distanza minima tra rette illimitate
      Vector3d vtStSt = ptSt2 - ptSt1 ;
      double dDist01 = vtStSt * vtD1 ;
      double dDist02 = vtStSt * vtD2 ;
      double dDotD1D2 = vtD1 * vtD2 ;
      double dT1 = ( dDist01 - dDotD1D2 * dDist02) / ( 1 - dDotD1D2 * dDotD1D2) ;
      double dT2 = ( dDotD1D2 * dDist01 - dDist02) / ( 1 - dDotD1D2 * dDotD1D2) ;
     // Posizioni minime e massime sui segmenti
      double dMin1 = ( bIsSegment1 ? 0 : -INFINITO) ;
      double dMax1 = ( bIsSegment1 ? dLen1 : INFINITO) ;
      double dMin2 = ( bIsSegment2 ? 0 : -INFINITO) ;
      double dMax2 = ( bIsSegment2 ? dLen2 : INFINITO) ;
     // Se entrambe le posizioni stanno nei segmenti 
      if ( dT1 >= dMin1  &&  dT1 <= dMax1  &&  dT2 >= dMin2  &&  dT2 <= dMax2) {
         m_dPos1 = dT1 ;
         m_dPos2 = dT2 ;
      }
     // se altrimenti solo la prima sta nel segmento
      else if ( dT1 >= dMin1  &&  dT1 <= dMax1) {
         m_dPos2 = Clamp( dT2, dMin2, dMax2) ;
         m_dPos1 = Clamp( (( ptSt2 + m_dPos2 * vtD2) - ptSt1) * vtD1, dMin1, dMax1) ;
      }
     // se altrimenti solo la seconda sta nel segmento
      else if ( dT2 >= dMin2  &&  dT2 <= dMax2) {
         m_dPos1 = Clamp( dT1, dMin1, dMax1) ;
         m_dPos2 = Clamp( (( ptSt1 + m_dPos1 * vtD1) - ptSt2) * vtD2, dMin2, dMax2) ;
      }
     // altrimenti nessuna sta nel suo segmento
      else {
         m_dPos1 = Clamp( dT1, dMin1, dMax1) ;
         m_dPos2 = Clamp( (( ptSt1 + m_dPos1 * vtD1) - ptSt2) * vtD2, dMin2, dMax2) ;
         m_dPos1 = Clamp( (( ptSt2 + m_dPos2 * vtD2) - ptSt1) * vtD1, dMin1, dMax1) ;
      }
      m_ptMinDist1 = ptSt1 + m_dPos1 * vtD1 ;
      m_ptMinDist2 = ptSt2 + m_dPos2 * vtD2 ;
      m_dSqDist = SqDist( m_ptMinDist1, m_ptMinDist2) ;
   }
}