//----------------------------------------------------------------------------
//  EgalTech 2020-2020
//----------------------------------------------------------------------------
// File : DistLineLine.h      Data : 06.11.20         Versione : 2.2k1
// Contenuto : Implementazione della classe distanza fra elementi lineari.
//
//
//
// Modifiche : 06.11.20 LM Creazione modulo.
//
//
//----------------------------------------------------------------------------

#include "stdafx.h"
#include "DistLineLine.h"
#include "/EgtDev/Include/EgtNumUtils.h"
#include "/EgtDev/Include/EGkGeoCollection.h"
#include "/EgtDev/Include/EGkGeoConst.h"
#include <algorithm>

//----------------------------------------------------------------------------
DistLineLine::DistLineLine( const Point3d& ptSt1, const Point3d& ptEn1,
                            const Point3d& ptSt2, const Point3d& ptEn2,
                            bool bIsSegment1, bool bIsSegment2)
{
   Vector3d vtD1 = ptEn1 - ptSt1 ;
   double dLen1 = vtD1.Len() ;
   Vector3d vtD2 = ptEn2 - ptSt2 ;
   double dLen2 = vtD1.Len() ;
   if ( dLen1 < EPS_SMALL  ||  dLen2 < EPS_SMALL) {
      m_dSqDist = - 1 ;
      m_dDist = - 1 ;
      return ;
   }
   vtD1 /= dLen1 ;
   vtD2 /= dLen2 ;
   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)
{
   if ( dLen1 < EPS_SMALL  ||  dLen2 < EPS_SMALL) {
      m_dSqDist = - 1 ;
      m_dDist = - 1 ;
      return ;
   }
   Calculate( ptSt1, vtD1, dLen1, ptSt2, vtD2, dLen2, bIsSegment1, bIsSegment2) ;
}

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

//----------------------------------------------------------------------------
bool
DistLineLine::GetDist( double& dDist)
{
   if ( m_dSqDist < 0)
      return false ;
   dDist = sqrt( m_dSqDist) ;
   return true ;
}

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

//----------------------------------------------------------------------------
bool
DistLineLine::GetParamsAtMinDistPoints( double& dPar1, double& dPar2)
{
   if ( m_dSqDist < 0)
      return false ;
   dPar1 = m_dPar1 ;
   dPar2 = m_dPar2 ;
   return true ;
}

//----------------------------------------------------------------------------
// Calcola la distanza fra i due elemnti lineari, i punti di minima distanza e
// i loro rispettivi parametri.
// Se la coppia di punti di minima distanza non è unica ne viene scelta una
// in base a comodità di calcolo.
void
DistLineLine::Calculate( const Point3d& ptSt1, const Vector3d& vtD1, double dLen1,
                         const Point3d& ptSt2, const Vector3d& vtD2, double dLen2,
                         bool bIsSegment1, bool bIsSegment2)
{
  // Caso di elementi lineari paralleli/antiparalleli
   if ( AreSameOrOppositeVectorExact( vtD1, vtD2)) {
     // Almeno un elemento è una retta
      if ( ! ( bIsSegment1  &&  bIsSegment2)) {
        // Il primo elemento è segmento, quindi deve essere una retta il secondo
         if ( bIsSegment1) {
            Vector3d vtStSt = ptSt1 - ptSt2 ;
            double dLong = vtStSt * vtD2 ;
            Vector3d vtDist = vtStSt - dLong * vtD2 ;
            m_dSqDist = vtDist.SqLen() ;
            m_dDist = sqrt( m_dSqDist) ;
            m_dPar1 = 0 ;
            m_dPar2 = dLong ;
            m_ptMinDist1 = ptSt1 ;
            m_ptMinDist2 = ptSt2 + dLong * vtD2 ;
         }
        // Il primo elemento è una retta
         else {
            Vector3d vtStSt = ptSt2 - ptSt1 ;
            double dLong = vtStSt * vtD1 ;
            Vector3d vtDist = vtStSt - dLong * vtD1 ;
            m_dSqDist = vtDist.SqLen() ;
            m_dDist = sqrt( m_dSqDist) ;
            m_dPar1 = dLong ;
            m_dPar2 = 0 ;
            m_ptMinDist1 = ptSt1 + dLong * vtD1 ;
            m_ptMinDist2 = ptSt2 ;
         }
      }
     // 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 segmento 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_dDist = sqrt( m_dSqDist) ;
            m_ptMinDist1 = ptEn1 ;
            m_ptMinDist2 = ptMinPar ;
            m_dPar1 = dLen1 ;
            m_dPar2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
         }
         else if ( dMinPar > 0) {
            m_dSqDist = std::max( vtStSt * vtStSt - dStU * dStU, 0.) ;
            m_dDist = sqrt( m_dSqDist) ;
            m_ptMinDist1 = ptSt1 + dMinPar * vtD1 ;
            m_ptMinDist2 = ptMinPar ;
            m_dPar1 = dMinPar ;
            m_dPar2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
         }
         else if ( dMaxPar > 0) {
            m_dSqDist = std::max( vtStSt * vtStSt - dStU * dStU, 0.) ;
            m_dDist = sqrt( m_dSqDist) ;
            m_ptMinDist1 = ptSt1 ;
            m_ptMinDist2 = ptSt2 + ( ptSt1 - ptSt2) * vtD2 * vtD2 ;
            m_dPar1 = 0 ;
            m_dPar2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
         }
         else {
            m_dSqDist = SqDist( ptSt1, ptMaxPar) ;
            m_dDist = sqrt( m_dSqDist) ;
            m_ptMinDist1 = ptSt1 ;
            m_ptMinDist2 = ptMaxPar ;
            m_dPar1 = 0 ;
            m_dPar2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
         }
      }
      return ;
   }
  // Caso generale
   Vector3d vtDist0 = ptSt2 - ptSt1 ;
   double dDist01 = vtDist0 * vtD1 ;
   double dDist02 = vtDist0 * vtD2 ;
   double dDotD1D2 = vtD1 * vtD2 ;
   double dT1 = dDist01 + ( ( dDist01 * dDotD1D2 - dDist02) * dDotD1D2) / ( 1 - dDotD1D2 * dDotD1D2) ;
   double dT2 = ( dDist01 * dDotD1D2 - dDist02) / ( 1 - dDotD1D2 * dDotD1D2) ;
   double dMin1 = - INFINITO ;
   double dMax1 = INFINITO ;
   double dMin2 = - INFINITO ;
   double dMax2 = INFINITO ;
   double dSt1On2 = ( ptSt1 - ptSt2) * vtD2 ;
   double dEn1On2 = ( ptSt1 + dLen1 * vtD1 - ptSt2) * vtD2 ;
   if ( bIsSegment1) {
      dMin1 = 0 ;
      dMax1 = dLen1 ;
      dMin2 = std::min( dSt1On2, dEn1On2) ;
      dMax2 = std::max( dSt1On2, dEn1On2) ;
   }
   if ( bIsSegment2) {
      double dSt2On1 = ( ptSt2 - ptSt1) * vtD1 ;
      double dEn2On1 = ( ptSt2 + dLen2 * vtD2 - ptSt1) * vtD1 ;
      dMin1 = std::max( dMin1, std::min( dSt2On1, dEn2On1)) ;
      dMax1 = std::min( dMax1, std::max( dSt2On1, dEn2On1)) ;
      dMin2 = std::max( dMin2, 0.) ;
      dMax2 = std::min( dMax2, dLen2) ;
   }
   m_dPar1 = Clamp( dT1, dMin1, dMax1) ;
   m_dPar2 = Clamp( dT2, dMin2, dMax2) ;
   m_ptMinDist1 = ptSt1 + m_dPar1 * vtD1 ;
   m_ptMinDist2 = ptSt2 + m_dPar2 * vtD2 ;
   m_dSqDist = SqDist( m_ptMinDist1, m_ptMinDist2) ;
   m_dDist = sqrt( m_dSqDist) ;
}