EgtGeomKernel/DistPointCrvBezier.cpp

//----------------------------------------------------------------------------
//  EgalTech 2013-2013
//----------------------------------------------------------------------------
// File : DistPointCrvBezier.cpp     Data : 02.01.14        Versione : 1.5a1
// Contenuto : Implementazione della classe distanza punto da curva di Bezier.
//
//
//
// Modifiche : 02.01.14 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "DistPointCrvBezier.h"
#include "DistPointLine.h"
#include "\EgtDev\Include\EgtTrace.h"


//----------------------------------------------------------------------------
DistPointCrvBezier::DistPointCrvBezier( const Point3d& ptP, const ICurveBezier& CrvBez)
{
   const double LIN_TOL_APPROX = 1 ;
   const double ANG_TOL_APPROX_DEG = 45 ;


  // distanza non calcolata
   m_dDist = - 1 ;

   if ( ! CrvBez.IsValid())
      return ;

  // creo una polilinea di approssimazione
   PolyLine PL ;
   if ( ! CrvBez.ApproxWithLines( LIN_TOL_APPROX, ANG_TOL_APPROX_DEG, PL))
      return ;

  // cerco la minima distanza per la polilinea
   double dSqDist ;
   double dMinPar ;
   double dMinParIni ;
   double dMinParFin ;
   double dPar ;
   double dUIni ;
   double dUFin ;
   Point3d ptMinDist ;
   Point3d ptIni ;
   Point3d ptFin ;
   bool bFound = false ;
   bool bOnEnd = false ;
   double dSqMinDist = INFINITO * INFINITO ;
   for ( bool bLine = PL.GetFirstULine( &dUIni, &ptIni, &dUFin, &ptFin) ;
         bLine ;
         bLine = PL.GetNextULine( &dUIni, &ptIni, &dUFin, &ptFin)) {
     // calcolo la distanza del punto dal segmento
      DistPointLine dstPtLn( ptP, ptIni, ptFin) ;
      if ( dstPtLn.GetSqDist( dSqDist)  &&  dSqDist < dSqMinDist) {
         bFound = true ;
         dSqMinDist = dSqDist ;
         dstPtLn.GetPointMinDist( ptMinDist) ;
         dstPtLn.GetParamAtPointMinDist( dPar) ;
         dMinPar = ( 1 - dPar) * dUIni + dPar * dUFin ;
         dMinParIni = dUIni ;
         dMinParFin = dUFin ;
         bOnEnd = ( AreSamePointNear( ptMinDist, ptFin)) ;
      }
      else if ( bOnEnd) {
         bOnEnd = false ;
         dMinParFin = dUFin ;
      }
   }
   if ( ! bFound)
      return ;

  // raffino i punti trovati (con algoritmo tipo Newton da TheNurbsBook pag 230)
   const int MAX_COUNT = 10 ;
   double dMinDist = sqrt( dSqMinDist) ;
   double dPrevPar ;
   double dTemp ;
   double dSqCosA ;
   Point3d ptQ ;
   Vector3d vtDer1 ;
   Vector3d vtDer2 ;
   Vector3d vtDiff ;
   int nCount = 0 ;
   dPar = dMinPar ;
   do {
     // contatore iterazioni
      nCount ++ ;
     // calcolo P, D1 e D2
      CrvBez.GetPointD1D2( dPar, ICurve::FROM_MINUS, ptQ, &vtDer1, &vtDer2) ;
     // se D1 nulla e distanza sicuramente non minima, riprovo sul centro dell'intervallo
      if ( vtDer1.IsZero()  &&  Dist( ptQ, ptP) > __min( 1.2 * dMinDist, dMinDist + LIN_TOL_APPROX)) {
         dPar = 0.5 * ( dMinParIni + dMinParFin) ;
         CrvBez.GetPointD1D2( dPar, ICurve::FROM_MINUS, ptQ, &vtDer1, &vtDer2) ;
      }
     // vettore dal punto al piede sulla curva
      vtDiff = ptQ - ptP ;
     // angolo tra vettore e tangente
      dTemp = vtDer1 * vtDiff ;
      if ( fabs( dTemp) > EPS_ZERO) 
         dSqCosA = dTemp * dTemp / ( vtDer1.SqLen() * vtDiff.SqLen()) ;
      else
         dSqCosA = 0 ;
     // stima prossimo valore del parametro (Newton : Unext = U - F(U) / F'(U))
      dPrevPar = dPar ;
      dTemp = vtDer2 * vtDiff + vtDer1.SqLen() ;
      if ( fabs( dTemp) > EPS_ZERO)
         dPar = dPrevPar - ( vtDer1 * vtDiff) / dTemp ;
     // clipping parametro in 0-1
      if ( dPar < dMinParIni)
         dPar = dMinParIni ;
      else if ( dPar > dMinParFin)
         dPar = dMinParFin ;
   }  while ( nCount < MAX_COUNT  &&  fabs( dPar - dPrevPar) > EPS_ZERO  &&
              fabs( dSqCosA) > COS_ORTO_ANG_ZERO * COS_ORTO_ANG_ZERO) ;

#if defined( _DEBUG)
   if ( nCount == MAX_COUNT)
      EGT_TRACE( "Exceeded MAX_COUNT in PolishingDistPointCrv.") ;
#endif

  // assegno i dati
   m_bDet = true ;
   m_dDist = Dist( ptP, ptQ) ;
   m_dParam = dPrevPar ;
   m_ptMinDist = ptQ ;
}

//----------------------------------------------------------------------------
bool
DistPointCrvBezier::GetSqDist( double& dSqDist)
{
   if ( m_dDist < 0)
      return false ;

   dSqDist = m_dDist * m_dDist ;
   return true ;
}

//----------------------------------------------------------------------------
bool
DistPointCrvBezier::GetDist( double& dDist)
{
   if ( m_dDist < 0)
      return false ;

   dDist = m_dDist ;
   return true ;
}

//----------------------------------------------------------------------------
bool
DistPointCrvBezier::GetPointMinDist( Point3d& ptMinDist, bool* pbDet)
{
   if ( m_dDist < 0)
      return false ;

   ptMinDist = m_ptMinDist ;
   if ( pbDet != nullptr)
      *pbDet = m_bDet ;
   return true ;
}

//----------------------------------------------------------------------------
bool
DistPointCrvBezier::GetParamAtPointMinDist( double& dParam, bool* pbDet)
{
   if ( m_dDist < 0)
      return false ;

   dParam = m_dParam ;
   if ( pbDet != nullptr)
      *pbDet = m_bDet ;
   return true ;
}