Daniele Bariletti
245 lines
9.3 KiB
C++
245 lines
9.3 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2013-2014
|
|
//----------------------------------------------------------------------------
|
|
// File : DistPointCrvAux.cpp Data : 13.01.14 Versione : 1.5a3
|
|
// Contenuto : Funzioni ausiliarie per calcolo distanza punto da curva.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 13.01.14 DS Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
//--------------------------- Include ----------------------------------------
|
|
#include "stdafx.h"
|
|
#include "DllMain.h"
|
|
#include "GeoConst.h"
|
|
#include "DistPointCrvAux.h"
|
|
#include "/EgtDev/Include/EGkDistPointLine.h"
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CalcMinDistPointPolyLine( const Point3d& ptP, PolyLine& PL, double dLinTol, MDCVECTOR& vApproxMin)
|
|
{
|
|
|
|
vApproxMin.reserve( 4) ;
|
|
vApproxMin.clear() ;
|
|
|
|
bool bFound = false ;
|
|
bool bOnEnd = false ;
|
|
double dUIni, dUFin ;
|
|
Point3d ptIni, ptFin ;
|
|
double dMinDist, 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) ;
|
|
double dSqDist ;
|
|
if ( ! dstPtLn.GetSqDist( dSqDist))
|
|
continue ;
|
|
// altro punto con la stessa minima distanza già trovata
|
|
if ( bFound && abs( dSqDist - dSqMinDist) < 2 * dMinDist * dLinTol) {
|
|
// salvo i dati nella struttura
|
|
MinDistCalc approxMin ;
|
|
approxMin.dDist = dMinDist ;
|
|
dstPtLn.GetMinDistPoint( approxMin.ptQ) ;
|
|
double dPar ;
|
|
dstPtLn.GetParamAtMinDistPoint( dPar) ;
|
|
approxMin.dPar = ( 1 - dPar) * dUIni + dPar * dUFin ;
|
|
approxMin.dParMin = dUIni ;
|
|
approxMin.dParMax = dUFin ;
|
|
// setto che il punto è alla fine
|
|
bOnEnd = ( AreSamePointApprox( 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
|
|
MinDistCalc approxMin ;
|
|
approxMin.dDist = dMinDist ;
|
|
dstPtLn.GetMinDistPoint( approxMin.ptQ) ;
|
|
double dPar ;
|
|
dstPtLn.GetParamAtMinDistPoint( dPar) ;
|
|
approxMin.dPar = ( 1 - dPar) * dUIni + dPar * dUFin ;
|
|
approxMin.dParMin = dUIni ;
|
|
approxMin.dParMax = dUFin ;
|
|
// setto che il punto è alla fine
|
|
bOnEnd = ( AreSamePointApprox( 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 ;
|
|
}
|
|
}
|
|
|
|
return bFound ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
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 = ( abs( dPar - approxMin.dParMin) < EPS_PARAM) ? 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 ;
|
|
bool bEquiverse = dTemp > 0 ;
|
|
if ( abs( 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() ;
|
|
|
|
// se il coseno tra questi due vettori è troppo grande potrei aver avuto una cattiva stima iniziale
|
|
// provo quindi ad aggiustare a mano, anziché usare il segno suggerito da newton, che con queste premesse potrebbe divergere
|
|
double dCos75 = 0.2588 ;
|
|
if ( abs( dTemp) > EPS_ZERO) {
|
|
double dDelta = ( vtDer1 * vtDiff) / dTemp ;
|
|
if ( dSqCosA > dCos75) {
|
|
if ( ( bEquiverse && dDelta > 0) || ( ! bEquiverse && dDelta < 0))
|
|
dDelta *= -1 ;
|
|
dPar = dPrevPar + dDelta ;
|
|
}
|
|
else
|
|
dPar = dPrevPar - dDelta ;
|
|
}
|
|
// 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 && abs( dPar - dPrevPar) > EPS_PARAM &&
|
|
abs( dSqCosA) > COS_ORTO_ANG_ZERO * COS_ORTO_ANG_ZERO) ;
|
|
|
|
if ( nCount == MAX_COUNT && GetEGkDebugLev() >= 5)
|
|
LOG_DBG_ERR( GetEGkLogger(), "ERROR : Exceeded recursions") ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
FilterMinDistPointCurve( const Point3d& ptP, const ICurve& cCurve,
|
|
const MDCVECTOR& vApproxMin, double& dMinDist, MDPCIVECTOR& Info)
|
|
{
|
|
// determino i minimi raffinati da tenere
|
|
bool bFound = false ;
|
|
bool bLastOnEnd = false ; // flag per ultimo punto su fine del suo intervallo
|
|
for ( auto Iter = vApproxMin.cbegin() ; Iter != vApproxMin.cend() ; ++Iter) {
|
|
// altro punto con la stessa minima distanza
|
|
if ( bFound && abs( (*Iter).dDist - dMinDist) < EPS_SMALL) {
|
|
// se abbastanza lontano e non su bordi intervallino lo aggiungo
|
|
if ( SqDist( (*Iter).ptQ, Info.back().ptQ) > MIN_LEN_CONT_MDPC &&
|
|
! bLastOnEnd && abs( (*Iter).dPar - (*Iter).dParMin) > EPS_PARAM) {
|
|
Info.emplace_back( MDPCI_NORMAL, (*Iter).dPar, (*Iter).ptQ) ;
|
|
bLastOnEnd = abs( (*Iter).dPar - (*Iter).dParMax) < EPS_PARAM ;
|
|
}
|
|
// altrimenti lo sostituisco se distanza minore
|
|
else if ( (*Iter).dDist < dMinDist) {
|
|
Info.back() = MinDistPCInfo( MDPCI_NORMAL, (*Iter).dPar, (*Iter).ptQ) ;
|
|
bLastOnEnd = abs( (*Iter).dPar - (*Iter).dParMax) < EPS_PARAM ;
|
|
}
|
|
}
|
|
// 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
|
|
Info.clear() ;
|
|
Info.emplace_back( MDPCI_NORMAL, (*Iter).dPar, (*Iter).ptQ) ;
|
|
bLastOnEnd = abs( (*Iter).dPar - (*Iter).dParMax) < EPS_PARAM ;
|
|
}
|
|
}
|
|
if ( Info.empty())
|
|
return false ;
|
|
|
|
// se 2 o più minimi, verifico se tratto continuo
|
|
if ( 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) Info.size() ; ++ i) {
|
|
dU = 0.5 * ( Info[i-1].dPar + Info[i].dPar) ;
|
|
cCurve.GetPointD1D2( dU, ICurve::FROM_MINUS, ptQ) ;
|
|
if ( abs( 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 ( ( Info.back().dPar - Info.front().dPar) > 0.8) {
|
|
// primo elemento == inizio della curva
|
|
Info[0].dPar = 0 ;
|
|
Info[0].nFlag = MDPCI_START_CONT ;
|
|
cCurve.GetStartPoint( Info[0].ptQ) ;
|
|
// ultimo elemento == fine curva
|
|
Info[1].dPar = 1 ;
|
|
Info[1].nFlag = MDPCI_END_CONT ;
|
|
cCurve.GetEndPoint( Info[1].ptQ) ;
|
|
}
|
|
else {
|
|
// sistemo flag primo elemento
|
|
Info[0].nFlag = MDPCI_START_CONT ;
|
|
// sposto ultimo elemento al secondo posto e ne sistemo il flag
|
|
Info[1] = Info.back() ;
|
|
Info[1].nFlag = MDPCI_END_CONT ;
|
|
}
|
|
// cancello tutti gli altri elementi
|
|
Info.erase( Info.begin() + 2, Info.end()) ;
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|