Egalware/EgtGeomKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
0ec9bceb166e6af78ad8104fa9e61c1f7b3bf2d2
EgtGeomKernel/DistPointCrvBezier.cpp
T
Dario Sassi 0ec9bceb16 EgtGeomKernel : Migliorata interfaccia calcolo distanza punto-curva, 
inoltre sistemata gestione per punti singoli e tratti continui
2014-01-13 21:11:19 +00:00

330 lines
12 KiB
C++
Raw Blame History

//----------------------------------------------------------------------------
// 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 "DllMain.h"
#include "DistPointCrvBezier.h"
#include "DistPointLine.h"
//----------------------------------------------------------------------------
static const double LIN_TOL_APPROX = 1 ;
static const double ANG_TOL_APPROX_DEG = 45 ;
//----------------------------------------------------------------------------
struct MinDistCalc {
double dDist ;
double dPar ;
double dParMin ;
double dParMax ;
bool bParMinSing ;
bool bParMaxSing ;
Point3d ptQ ;
MinDistCalc( void)
: dDist( 0), dPar( 0), dParMin( 0), dParMax( 0),
bParMinSing( false), bParMaxSing( false), ptQ( 0, 0, 0) {}
MinDistCalc( double dD, double dP, double dPm, double dPM, Point3d pT)
: dDist( dD), dPar( dP), dParMin( dPm), dParMax( dPM),
bParMinSing( false), bParMaxSing( false), ptQ( pT) {}
MinDistCalc( double dD, double dP, double dPm, double dPM, bool bMinS, bool bMaxS, Point3d pT)
: dDist( dD), dPar( dP), dParMin( dPm), dParMax( dPM),
bParMinSing( bMinS), bParMaxSing( bMaxS), ptQ( pT) {}
} ;
typedef std::vector<MinDistCalc> MDCVECTOR ; // vettore di MinDistCalc
//----------------------------------------------------------------------------
bool
PolishMinDistPointCurve( const Point3d& ptP, const ICurve& cCurve,
const MinDistCalc& approxMin, double& dPrevPar, Point3d& ptQ)
{
const int MAX_COUNT = 16 ;
ICurve::Side nSide ;
double dPar ;
double dTemp ;
double dSqCosA ;
Vector3d vtDer1 ;
Vector3d vtDer2 ;
Vector3d vtDiff ;
// raffino i punti trovati (con algoritmo tipo Newton da TheNurbsBook pag 230)
int nCount = 0 ;
bool bClampedFromSing = false ;
dPar = approxMin.dPar ;
do {
// contatore iterazioni
nCount ++ ;
// calcolo P, D1 e D2
nSide = ( fabs( dPar - approxMin.dParMin) < EPS_ZERO) ? ICurve::FROM_PLUS : ICurve::FROM_MINUS ;
cCurve.GetPointD1D2( dPar, nSide, 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
if ( dPar < approxMin.dParMin) {
if ( approxMin.bParMinSing && ! bClampedFromSing) {
dPar = approxMin.dParMax ;
bClampedFromSing = true ;
}
else
dPar = approxMin.dParMin ;
}
else if ( dPar > approxMin.dParMax) {
if ( approxMin.bParMaxSing && ! bClampedFromSing) {
dPar = approxMin.dParMin ;
bClampedFromSing = true ;
}
else
dPar = approxMin.dParMax ;
}
} while ( nCount < MAX_COUNT && fabs( dPar - dPrevPar) > EPS_ZERO &&
fabs( dSqCosA) > COS_ORTO_ANG_ZERO * COS_ORTO_ANG_ZERO) ;
if ( nCount == MAX_COUNT)
LOG_DBG_ERR( GetEGkLogger(), "ERROR : Exceeded recursions") ;
return true ;
}
//----------------------------------------------------------------------------
DistPointCrvBezier::DistPointCrvBezier( const Point3d& ptP, const ICurveBezier& CrvBez)
{
// 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
MinDistCalc approxMin ;
MDCVECTOR vApproxMin ;
MDCVECTOR::iterator Iter ;
vApproxMin.reserve( 4) ;
double dSqDist ;
double dPar ;
double dUIni ;
double dUFin ;
Point3d ptIni ;
Point3d ptFin ;
bool bFound = false ;
bool bOnEnd = false ;
double dMinDist ;
double dSqMinDist ;
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))
continue ;
// altro punto con la stessa minima distanza già trovata
if ( bFound && fabs( dSqDist - dSqMinDist) < 2 * dMinDist * LIN_TOL_APPROX) {
// salvo i dati nella struttura
approxMin.dDist = dMinDist ;
dstPtLn.GetMinDistPoint( approxMin.ptQ) ;
dstPtLn.GetParamAtMinDistPoint( dPar) ;
approxMin.dPar = ( 1 - dPar) * dUIni + dPar * dUFin ;
approxMin.dParMin = dUIni ;
approxMin.dParMax = dUFin ;
// setto che il punto è alla fine
bOnEnd = ( AreSamePointNear( approxMin.ptQ, ptFin)) ;
// aggiungo alla lista
vApproxMin.push_back( approxMin) ;
}
// primo punto o punto con minima distanza più bassa
else if ( ! bFound || dSqDist < dSqMinDist) {
// aggiorno i minimi
bFound = true ;
dSqMinDist = dSqDist ;
dMinDist = sqrt( dSqMinDist) ;
// salvo i dati nella struttura
approxMin.dDist = dMinDist ;
dstPtLn.GetMinDistPoint( approxMin.ptQ) ;
dstPtLn.GetParamAtMinDistPoint( dPar) ;
approxMin.dPar = ( 1 - dPar) * dUIni + dPar * dUFin ;
approxMin.dParMin = dUIni ;
approxMin.dParMax = dUFin ;
// setto che il punto è alla fine
bOnEnd = ( AreSamePointNear( approxMin.ptQ, ptFin)) ;
// il nuovo vettore deve contenere solo quest'ultimo minimo
vApproxMin.clear() ;
vApproxMin.push_back( approxMin) ;
}
// il minimo era alla fine, devo allargare l'intervallo di raffinamento del parametro
else if ( bOnEnd) {
bOnEnd = false ;
vApproxMin.back().dParMax = dUFin ;
}
}
if ( ! bFound)
return ;
// verifico presenza singolarità agli estremi degli intervalli trovati
double dSingP ;
if ( CrvBez.GetSingularParam( dSingP) == 0)
dSingP = - 1 ;
for ( Iter = vApproxMin.begin() ; Iter != vApproxMin.end() ; ++Iter) {
// imposto flag per singolarità agli estremi
(*Iter).bParMinSing = fabs( (*Iter).dParMin - dSingP) < EPS_SMALL ;
(*Iter).bParMaxSing = fabs( (*Iter).dParMax - dSingP) < EPS_SMALL ;
}
// raffino i punti trovati
double dPolishedPar ;
Point3d ptPolishedQ ;
for ( Iter = vApproxMin.begin() ; Iter != vApproxMin.end() ; ++Iter) {
// eseguo raffinamento
if ( PolishMinDistPointCurve( ptP, CrvBez, *Iter, dPolishedPar, ptPolishedQ)) {
(*Iter).dDist = Dist( ptP, ptPolishedQ) ;
(*Iter).dPar = dPolishedPar ;
(*Iter).ptQ = ptPolishedQ ;
}
else
(*Iter).dDist = INFINITO ;
}
// determino i minimi raffinati da tenere
bFound = false ;
for ( Iter = vApproxMin.begin() ; Iter != vApproxMin.end() ; ++Iter) {
// altro punto con la stessa minima distanza
if ( bFound && fabs( (*Iter).dDist - dMinDist) < EPS_SMALL) {
// se abbastanza lontano lo aggiungo
if ( SqDist( (*Iter).ptQ, m_Info.back().ptQ) > 1)
m_Info.push_back( MinDistPCInfo( MDPCI_NORMAL, (*Iter).dPar, (*Iter).ptQ)) ;
// altrimenti lo sostituisco se distanza minore
else if ( (*Iter).dDist < dMinDist)
m_Info.back() = MinDistPCInfo( MDPCI_NORMAL, (*Iter).dPar, (*Iter).ptQ) ;
}
// primo punto o punto con minima distanza più bassa
else if ( ! bFound || (*Iter).dDist < dMinDist) {
// aggiorno i minimi
bFound = true ;
dMinDist = (*Iter).dDist ;
// il nuovo vettore deve contenere solo quest'ultimo minimo
m_Info.clear() ;
m_Info.push_back( MinDistPCInfo( MDPCI_NORMAL, (*Iter).dPar, (*Iter).ptQ)) ;
}
}
if ( m_Info.empty())
return ;
// se 2 o più minimi, verifico se tratto continuo
if ( m_Info.size() >= 2) {
bool bCont = true ;
double dU ;
Point3d ptQ ;
// se tutti i punti intermedi hanno la stessa distanza, è una zona continua
for ( int i = 1 ; i < (int) m_Info.size() ; ++ i) {
dU = 0.5 * ( m_Info[i-1].dPar + m_Info[i].dPar) ;
CrvBez.GetPointD1D2( dU, ICurve::FROM_MINUS, ptQ) ;
if ( fabs( SqDist( ptP, ptQ) - dMinDist * dMinDist) > 2 * dMinDist * EPS_SMALL) {
bCont = false ;
break ;
}
}
// se zona continua è un arco di circonferenza, tengo solo primo e ultimo punto e imposto opportuni flag
if ( bCont) {
// se è praticamente tutta la curva, la faccio diventare tutta
if ( ( m_Info.back().dPar - m_Info.front().dPar) > 0.8) {
// primo elemento == inizio della curva
m_Info[0].dPar = 0 ;
m_Info[0].nFlag = MDPCI_START_CONT ;
CrvBez.GetStartPoint( m_Info[0].ptQ) ;
// ultimo elemento == fine curva
m_Info[1].dPar = 1 ;
m_Info[1].nFlag = MDPCI_END_CONT ;
CrvBez.GetEndPoint( m_Info[1].ptQ) ;
}
else {
// sistemo flag primo elemento
m_Info[0].nFlag = MDPCI_START_CONT ;
// sposto ultimo elemento al secondo posto e ne sistemo il flag
m_Info[1] = m_Info.back() ;
m_Info[1].nFlag = MDPCI_END_CONT ;
}
// cancello tutti gli altri elementi
m_Info.erase( m_Info.begin() + 2, m_Info.end()) ;
}
}
// salvo anche il valore della minima distanza
m_dDist = dMinDist ;
}
//----------------------------------------------------------------------------
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::GetMinDistPoint( int nInd, Point3d& ptMinDist, int& nFlag)
{
if ( m_dDist < 0)
return false ;
if ( nInd < 0 || nInd >= (int) m_Info.size())
return false ;
ptMinDist = m_Info[nInd].ptQ ;
nFlag = m_Info[nInd].nFlag ;
return true ;
}
//----------------------------------------------------------------------------
bool
DistPointCrvBezier::GetParamAtMinDistPoint( int nInd, double& dParam, int& nFlag)
{
if ( m_dDist < 0)
return false ;
if ( nInd < 0 || nInd >= (int) m_Info.size())
return false ;
dParam = m_Info[nInd].dPar ;
nFlag = m_Info[nInd].nFlag ;
return true ;
}
Reference in New Issue View Git Blame Copy Permalink