Egalware/EgtGeomKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
974d05002540d834fc923ff54417fd790ab8e678
EgtGeomKernel/CAvSimpleSurfFrMove.cpp
T
Dario Sassi 3a3c591771 EgtGeomKernel 1.9e1 : 
- prima versione di Collision Avoiding per Utensili rispetto a Superfici TriMesh.
2018-04-30 14:36:00 +00:00

685 lines
27 KiB
C++
Raw Blame History

//----------------------------------------------------------------------------
// EgalTech 2015-2018
//----------------------------------------------------------------------------
// File : CASimpleSurfFrMove.cpp Data : 27.04.18 Versione : 1.9e1
// Contenuto : Implementazione delle funzioni di movimento per SurfFlatRegion
// senza collisione con altri oggetti dello stesso tipo e nello
// stesso piano o in piani paralleli.
//
//
// Modifiche : 29.12.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------
//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "CAvSimpleSurfFrMove.h"
#include "SurfFlatRegion.h"
#include "CurveLine.h"
#include "CurveArc.h"
#include "CurveComposite.h"
#include "IntersLineArc.h"
#include "GeoConst.h"
#include "/EgtDev/Include/EGkCAvSimpleSurfFrMove.h"
#include "/EgtDev/Include/EgtPointerOwner.h"
using namespace std ;
//----------------------------------------------------------------------------
// CASimpleSurfFrMove
//----------------------------------------------------------------------------
CAvSimpleSurfFrMove::CAvSimpleSurfFrMove( const ISurfFlatRegion& SfrM, const ISurfFlatRegion& SfrF)
{
// salvo puntatori alle regioni
m_pRegM = &SfrM ;
m_pRegF = &SfrF ;
}
//----------------------------------------------------------------------------
bool
CAvSimpleSurfFrMove::Translate( const Vector3d& vtDir, double& dLen)
{
MyCAvSimpleSurfFrMove ScdMove ( *m_pRegM, *m_pRegF) ;
m_SCollInfo.nType = SCI_NONE ;
if ( ! ScdMove.Translate( vtDir, dLen))
return false ;
m_SCollInfo = ScdMove.GetSCollInfo() ;
return true ;
}
//----------------------------------------------------------------------------
bool
CAvSimpleSurfFrMove::Rotate( const Point3d& ptCen, double& dAng)
{
MyCAvSimpleSurfFrMove ScdMove ( *m_pRegM, *m_pRegF) ;
m_SCollInfo.nType = SCI_NONE ;
return ScdMove.Rotate( ptCen, dAng) ;
}
//----------------------------------------------------------------------------
// MyCAvSimpleSurfFrMove
//----------------------------------------------------------------------------
MyCAvSimpleSurfFrMove::MyCAvSimpleSurfFrMove( const ISurfFlatRegion& SfrM, const ISurfFlatRegion& SfrF)
{
// recupero rappresentazione base della regione mobile
m_pRegM = GetBasicSurfFlatRegion( &SfrM) ;
// ne verifico lo stato
if ( m_pRegM == nullptr || ! m_pRegM->IsValid())
m_pRegM = nullptr ;
// recupero rappresentazione base della regione fissa
m_pRegF = GetBasicSurfFlatRegion( &SfrF) ;
// ne verifico lo stato
if ( m_pRegF == nullptr || ! m_pRegF->IsValid())
m_pRegF = nullptr ;
}
//----------------------------------------------------------------------------
bool
MyCAvSimpleSurfFrMove::Translate( const Vector3d& vtDir, double& dLen)
{
// verifico validità regioni
if ( m_pRegM == nullptr || m_pRegF == nullptr)
return false ;
// verifico che le due regioni giacciano in piani paralleli
if ( ! AreSameVectorApprox( m_pRegM->m_frF.VersZ(), m_pRegF->m_frF.VersZ()))
return false ;
// reset info di collisione
m_SCollInfo.nType = SCI_NONE ;
// porto il vettore di movimento nel riferimento intrinseco e ne annullo la componente Z
Vector3d vtDirL = vtDir ;
vtDirL.ToLoc( m_pRegM->m_frF) ;
vtDirL.z = 0 ;
double dLenXY = vtDirL.Len() ;
if ( dLenXY < EPS_SMALL)
return true ;
vtDirL /= dLenXY ;
dLenXY *= dLen ;
double dNewLenXY = dLenXY ;
// ciclo sui chunk della seconda superficie
for ( int i = 0 ; i < m_pRegF->GetChunkCount() ; ++ i) {
// curva esterna del chunk della seconda regione in locale nel riferimento intrinseco della prima
const ICurve* pCrv2Loc = nullptr ;
PtrOwner<ICurve> pCopyCrv ;
if ( AreSameFrame( m_pRegM->m_frF, m_pRegF->m_frF))
pCrv2Loc = m_pRegF->GetMyLoop( i, 0) ;
else {
pCopyCrv.Set( m_pRegF->GetMyLoop( i, 0)->Clone()) ;
if ( IsNull( pCopyCrv))
return REGC_NULL ;
pCopyCrv->LocToLoc( m_pRegF->m_frF, m_pRegM->m_frF) ;
pCrv2Loc = pCopyCrv ;
}
// ciclo sui chunk della prima superficie
for ( int j = 0 ; j < m_pRegM->GetChunkCount() ; ++ j) {
// curva esterna del chunk della prima regione (ovviamente già in locale al riferimento intrinseco)
const ICurve* pCrv1Loc = m_pRegM->GetMyLoop( j, 0) ;
// verifico la collisione tra le entità dei loop esterni dei due chunk
const CurveComposite* pCompo1 = GetBasicCurveComposite( pCrv1Loc) ;
const CurveComposite* pCompo2 = GetBasicCurveComposite( pCrv2Loc) ;
int k = 0 ;
const ICurve* pCrv1 = ( pCompo1 != nullptr ? pCompo1->GetFirstCurve() : pCrv1Loc) ;
while ( pCrv1 != nullptr) {
int l = 0 ;
const ICurve* pCrv2 = ( pCompo2 != nullptr ? pCompo2->GetFirstCurve() : pCrv2Loc) ;
while ( pCrv2 != nullptr) {
SCollInfo scInfoCurr ;
double dPrevLenXY = dNewLenXY ;
if ( ! TranslateCurveNoCollisionCurve( pCrv1, pCrv2, vtDirL, dNewLenXY, scInfoCurr))
return false ;
if ( abs( dNewLenXY - dPrevLenXY) < EPS_SMALL) {
if ( scInfoCurr.nType == SCI_LINE_LINE || scInfoCurr.nType == SCI_PNT_LINE) {
m_SCollInfo = scInfoCurr ;
m_SCollInfo.nChunkM = j ;
m_SCollInfo.nCrvM = k ;
m_SCollInfo.nChunkF = i ;
m_SCollInfo.nCrvF = l ;
}
}
else if ( dNewLenXY < dPrevLenXY) {
m_SCollInfo = scInfoCurr ;
m_SCollInfo.nChunkM = j ;
m_SCollInfo.nCrvM = k ;
m_SCollInfo.nChunkF = i ;
m_SCollInfo.nCrvF = l ;
}
pCrv2 = ( pCompo2 != nullptr ? pCompo2->GetNextCurve() : nullptr) ;
++ l ;
}
pCrv1 = ( pCompo1 != nullptr ? pCompo1->GetNextCurve() : nullptr) ;
++ k ;
}
}
}
// se da limitare il movimento
if ( dNewLenXY < dLenXY - EPS_SMALL)
dLen *= dNewLenXY / dLenXY ;
// porto i punti e le direzioni di SCollInfo da intrinseco a locale della prima regione
if ( m_SCollInfo.nType != SCI_NONE) {
m_SCollInfo.ptP1.ToGlob( m_pRegM->m_frF) ;
m_SCollInfo.vtDirM.ToGlob( m_pRegM->m_frF) ;
m_SCollInfo.vtDirF.ToGlob( m_pRegM->m_frF) ;
}
if ( m_SCollInfo.nType == SCI_LINE_LINE)
m_SCollInfo.ptP2.ToGlob( m_pRegM->m_frF) ;
return true ;
}
//----------------------------------------------------------------------------
bool
MyCAvSimpleSurfFrMove::Rotate( const Point3d& ptCen, double& dAng)
{
// verifico validità regioni
if ( m_pRegM == nullptr || m_pRegF == nullptr)
return false ;
// verifico che le due regioni giacciano in piani paralleli
if ( ! AreSameVectorApprox( m_pRegM->m_frF.VersZ(), m_pRegF->m_frF.VersZ()))
return false ;
// reset info di collisione
m_SCollInfo.nType = SCI_NONE ;
// porto il centro di rotazione nel riferimento intrinseco e ne annullo la componente Z
Point3d ptCenL = ptCen ;
ptCenL.ToLoc( m_pRegM->m_frF) ;
ptCenL.z = 0 ;
if ( abs( dAng) < EPS_ANG_SMALL)
return true ;
double dNewAng = dAng ;
// ciclo sui chunk della seconda superficie
for ( int i = 0 ; i < m_pRegF->GetChunkCount() ; ++ i) {
// curva esterna del chunk della seconda regione in locale nel riferimento intrinseco della prima
const ICurve* pCrv2Loc = nullptr ;
PtrOwner<ICurve> pCopyCrv ;
if ( AreSameFrame( m_pRegM->m_frF, m_pRegF->m_frF))
pCrv2Loc = m_pRegF->GetMyLoop( i, 0) ;
else {
pCopyCrv.Set( m_pRegF->GetMyLoop( i, 0)->Clone()) ;
if ( IsNull( pCopyCrv))
return REGC_NULL ;
pCopyCrv->LocToLoc( m_pRegF->m_frF, m_pRegM->m_frF) ;
pCrv2Loc = pCopyCrv ;
}
// ciclo sui chunk della prima superficie
for ( int j = 0 ; j < m_pRegM->GetChunkCount() ; ++ j) {
// curva esterna del chunk della prima regione (ovviamente già in locale al riferimento intrinseco)
const ICurve* pCrv1Loc = m_pRegM->GetMyLoop( j, 0) ;
// verifico la collisione tra le entità dei loop esterni dei due chunk
const CurveComposite* pCompo1 = GetBasicCurveComposite( pCrv1Loc) ;
const CurveComposite* pCompo2 = GetBasicCurveComposite( pCrv2Loc) ;
const ICurve* pCrv1 = ( pCompo1 != nullptr ? pCompo1->GetFirstCurve() : pCrv1Loc) ;
while ( pCrv1 != nullptr) {
const ICurve* pCrv2 = ( pCompo2 != nullptr ? pCompo2->GetFirstCurve() : pCrv2Loc) ;
while ( pCrv2 != nullptr) {
if ( ! RotateCurveNoCollisionCurve( pCrv1, pCrv2, ptCenL, dNewAng))
return false ;
pCrv2 = ( pCompo2 != nullptr ? pCompo2->GetNextCurve() : nullptr) ;
}
pCrv1 = ( pCompo1 != nullptr ? pCompo1->GetNextCurve() : nullptr) ;
}
}
}
// se da limitare il movimento
if ( ( dAng > 0 && dNewAng < dAng - EPS_ANG_SMALL) ||
( dAng < 0 && dNewAng > dAng + EPS_ANG_SMALL))
dAng = dNewAng ;
return true ;
}
//----------------------------------------------------------------------------
// Massima traslazione da posizione sicura in direzione e con limite dati
//----------------------------------------------------------------------------
bool
MyCAvSimpleSurfFrMove::TranslateCurveNoCollisionCurve( const ICurve* pCrv1, const ICurve* pCrv2,
const Vector3d& vtDir, double& dLen, SCollInfo& scInfo)
{
// se entrambe linee, procedo direttamente
if ( pCrv1->GetType() == CRV_LINE && pCrv2->GetType() == CRV_LINE) {
const CurveLine* pLine1 = GetBasicCurveLine( pCrv1) ;
const CurveLine* pLine2 = GetBasicCurveLine( pCrv2) ;
if ( ! TranslateLineNoCollisionLine( pLine1, pLine2, vtDir, dLen, scInfo))
return false ;
scInfo.vtDirM = pLine1->GetEnd() - pLine1->GetStart() ;
scInfo.vtDirF = pLine2->GetEnd() - pLine2->GetStart() ;
return true ;
}
// altrimenti confronto le approssimazioni con linee delle curve
// determino le due polilinee
PolyLine PL1 ;
if ( ! pCrv1->ApproxWithLines( LIN_TOL_FINE, ANG_TOL_STD_DEG, ICurve::APL_RIGHT, PL1))
return false ;
PolyLine PL2 ;
if ( ! pCrv2->ApproxWithLines( LIN_TOL_FINE, ANG_TOL_STD_DEG, ICurve::APL_RIGHT, PL2))
return false ;
// ciclo sulle linee della prima polilinea
Point3d ptStart1, ptEnd1 ;
bool bPL1 = PL1.GetFirstLine( ptStart1, ptEnd1) ;
while ( bPL1) {
// definisco la prima linea della coppia
CurveLine Line1 ;
Line1.Set( ptStart1, ptEnd1) ;
// ciclo sulle linee della seconda polilinea
Point3d ptStart2, ptEnd2 ;
bool bPL2 = PL2.GetFirstLine( ptStart2, ptEnd2) ;
while ( bPL2) {
// definisco la seconda linea della coppia
CurveLine Line2 ;
Line2.Set( ptStart2, ptEnd2) ;
// confronto le linee
SCollInfo scInfoCurr ;
double dPrevLen = dLen ;
if ( ! TranslateLineNoCollisionLine( &Line1, &Line2, vtDir, dLen, scInfoCurr))
return false ;
if ( dLen < dPrevLen) {
scInfo = scInfoCurr ;
scInfo.nType = SCI_PNT_PNT ;
scInfo.vtDirM = ptEnd1 - ptStart1 ;
scInfo.vtDirF = ptEnd2 - ptStart2 ;
}
bPL2 = PL2.GetNextLine( ptStart2, ptEnd2) ;
}
bPL1 = PL1.GetNextLine( ptStart1, ptEnd1) ;
}
return true ;
}
//----------------------------------------------------------------------------
// Massima traslazione da posizione sicura in direzione e con limite dati
//----------------------------------------------------------------------------
bool
MyCAvSimpleSurfFrMove::TranslateLineNoCollisionLine( const CurveLine* pLine1, const CurveLine* pLine2,
const Vector3d& vtDir, double& dLen, SCollInfo& scInfo)
{
// versore ortogonale al movimento
Vector3d vtNorm( vtDir.y, - vtDir.x, 0) ;
// coordinate degli estremi dei segmenti nel riferimento formato da O=L1Start X=vtNorm Y=vtDir
Point3d ptS1( 0, 0, 0) ;
Point3d ptE1( ( pLine1->GetEnd() - pLine1->GetStart()) * vtNorm,
( pLine1->GetEnd() - pLine1->GetStart()) * vtDir, 0) ;
Point3d ptS2( ( pLine2->GetStart() - pLine1->GetStart()) * vtNorm,
( pLine2->GetStart() - pLine1->GetStart()) * vtDir, 0) ;
Point3d ptE2( ( pLine2->GetEnd() - pLine1->GetStart()) * vtNorm,
( pLine2->GetEnd() - pLine1->GetStart()) * vtDir, 0) ;
// perpendicolari esterne alle due linee (nel riferimento sopra definito)
Vector3d vtN1( ( ptE1.y - ptS1.y), - ( ptE1.x - ptS1.x), 0) ;
Vector3d vtN2( ( ptE2.y - ptS2.y), - ( ptE2.x - ptS2.x), 0) ;
// reset info di collisione
scInfo.nType = SCI_NONE ;
// eseguo culling (le linee sono il contorno di una area chiusa)
if ( vtN1.y <= 0 || vtN2.y >= 0)
return true ;
// ordino l'intervallo della prima linea secondo X crescenti
if ( ptS1.x > ptE1.x)
swap( ptS1, ptE1) ;
// ordino l'intervallo della seconda linea secondo X crescenti
if ( ptS2.x > ptE2.x)
swap( ptS2, ptE2) ;
// se gli intervalli non si sovrappongono, non ci sono limiti
if ( ptS1.x > ptE2.x - EPS_SMALL || ptS2.x > ptE1.x - EPS_SMALL)
return true ;
// calcolo il minimo movimento degli estremi interni all'intervallo comune
double dNewLen ;
// estremo inferiore
if ( ptS1.x >= ptS2.x) {
double dY2 ;
if ( abs( ptE2.x - ptS2.x) < EPS_SMALL)
dY2 = min( ptE2.y, ptS2.y) ;
else
dY2 = ptS2.y + ( ptE2.y - ptS2.y) / ( ptE2.x - ptS2.x) * ( ptS1.x - ptS2.x) ;
dNewLen = dY2 - ptS1.y ;
if ( abs( ptS1.x - ptS2.x) < EPS_SMALL || abs( ptS1.x - ptE2.x) < EPS_SMALL)
scInfo.nType = SCI_PNT_PNT ;
else
scInfo.nType = SCI_PNT_LINE ;
scInfo.ptP1 = pLine1->GetStart() + ptS1.x * vtNorm + dY2 * vtDir ;
}
else {
double dY1 ;
if ( abs( ptE1.x - ptS1.x) < EPS_SMALL)
dY1 = max( ptE1.y, ptS1.y) ;
else
dY1 = ptS1.y + ( ptE1.y - ptS1.y) / ( ptE1.x - ptS1.x) * ( ptS2.x - ptS1.x) ;
dNewLen = ptS2.y - dY1 ;
if ( abs( ptS2.x - ptS1.x) < EPS_SMALL || abs( ptS2.x - ptE1.x) < EPS_SMALL)
scInfo.nType = SCI_PNT_PNT ;
else
scInfo.nType = SCI_LINE_PNT ;
scInfo.ptP1 = pLine1->GetStart() + ptS2.x * vtNorm + ptS2.y * vtDir ;
}
// estremo superiore
if ( ptE1.x <= ptE2.x) {
double dY2 ;
if ( abs( ptE2.x - ptS2.x) < EPS_SMALL)
dY2 = min( ptE2.y, ptS2.y) ;
else
dY2 = ptS2.y + ( ptE2.y - ptS2.y) / ( ptE2.x - ptS2.x) * ( ptE1.x - ptS2.x) ;
double dCurLen = dY2 - ptE1.y ;
if ( abs( dCurLen - dNewLen) < EPS_SMALL) {
scInfo.nType = SCI_LINE_LINE ;
scInfo.ptP2 = pLine1->GetStart() + ptE1.x * vtNorm + dY2 * vtDir ;
}
else if ( dCurLen < dNewLen) {
dNewLen = dCurLen ;
if ( abs( ptE1.x - ptS2.x) < EPS_SMALL || abs( ptE1.x - ptE2.x) < EPS_SMALL)
scInfo.nType = SCI_PNT_PNT ;
else
scInfo.nType = SCI_PNT_LINE ;
scInfo.ptP1 = pLine1->GetStart() + ptE1.x * vtNorm + dY2 * vtDir ;
}
}
else {
double dY1 ;
if ( abs( ptE1.x - ptS1.x) < EPS_SMALL)
dY1 = max( ptE1.y, ptS1.y) ;
else
dY1 = ptS1.y + ( ptE1.y - ptS1.y) / ( ptE1.x - ptS1.x) * ( ptE2.x - ptS1.x) ;
double dCurLen = ptE2.y - dY1 ;
if ( abs( dCurLen - dNewLen) < EPS_SMALL) {
scInfo.nType = SCI_LINE_LINE ;
scInfo.ptP2 = pLine1->GetStart() + ptE2.x * vtNorm + ptE2.y * vtDir ;
}
else if ( dCurLen < dNewLen) {
dNewLen = dCurLen ;
if ( abs( ptE2.x - ptS1.x) < EPS_SMALL || abs( ptE2.x - ptE1.x) < EPS_SMALL)
scInfo.nType = SCI_PNT_PNT ;
else
scInfo.nType = SCI_LINE_PNT ;
scInfo.ptP1 = pLine1->GetStart() + ptE2.x * vtNorm + ptE2.y * vtDir ;
}
}
// confronto con movimento corrente
if ( dNewLen > - 5 * EPS_SMALL && dNewLen < EPS_SMALL)
dLen = 0 ;
else if ( dNewLen > 0 && dNewLen < dLen)
dLen = dNewLen ;
else // non c'è collisione, reset Info
scInfo.nType = SCI_NONE ;
return true ;
}
//----------------------------------------------------------------------------
// Massima rotazione da posizione sicura in direzione e con limite dati
//----------------------------------------------------------------------------
bool
MyCAvSimpleSurfFrMove::RotateCurveNoCollisionCurve( const ICurve* pCrv1, const ICurve* pCrv2,
const Point3d& ptCen, double& dAng)
{
// se entrambe linee, procedo direttamente
if ( pCrv1->GetType() == CRV_LINE && pCrv2->GetType() == CRV_LINE) {
const CurveLine* pLine1 = GetBasicCurveLine( pCrv1) ;
const CurveLine* pLine2 = GetBasicCurveLine( pCrv2) ;
if ( ! RotateLineNoCollisionLine( pLine1, pLine2, ptCen, dAng))
return false ;
return true ;
}
// altrimenti confronto le approssimazioni con linee delle curve
// determino le due polilinee
PolyLine PL1 ;
if ( ! pCrv1->ApproxWithLines( LIN_TOL_FINE, ANG_TOL_STD_DEG, ICurve::APL_RIGHT, PL1))
return false ;
PolyLine PL2 ;
if ( ! pCrv2->ApproxWithLines( LIN_TOL_FINE, ANG_TOL_STD_DEG, ICurve::APL_RIGHT, PL2))
return false ;
// ciclo sulle linee della prima polilinea
Point3d ptStart1, ptEnd1 ;
bool bPL1 = PL1.GetFirstLine( ptStart1, ptEnd1) ;
while ( bPL1) {
// definisco la prima linea della coppia
CurveLine Line1 ;
Line1.Set( ptStart1, ptEnd1) ;
// ciclo sulle linee della seconda polilinea
Point3d ptStart2, ptEnd2 ;
bool bPL2 = PL2.GetFirstLine( ptStart2, ptEnd2) ;
while ( bPL2) {
// definisco la seconda linea della coppia
CurveLine Line2 ;
Line2.Set( ptStart2, ptEnd2) ;
// confronto le linee
if ( ! RotateLineNoCollisionLine( &Line1, &Line2, ptCen, dAng))
return false ;
bPL2 = PL2.GetNextLine( ptStart2, ptEnd2) ;
}
bPL1 = PL1.GetNextLine( ptStart1, ptEnd1) ;
}
return true ;
}
//----------------------------------------------------------------------------
// Massima rotazione da posizione sicura in direzione e con limite dati
//----------------------------------------------------------------------------
bool
MyCAvSimpleSurfFrMove::RotateLineNoCollisionLine( const CurveLine* pLine1, const CurveLine* pLine2,
const Point3d& ptCen, double& dAng)
{
// senso di rotazione
bool bCCW = ( dAng > 0) ;
// analisi della prima linea
Point3d ptS1 = pLine1->GetStart() ;
Point3d ptE1 = pLine1->GetEnd() ;
Vector3d vtDir1 = ptE1 - ptS1 ;
vtDir1.z = 0 ;
double dLen1 = vtDir1.LenXY() ;
if ( dLen1 < EPS_SMALL)
return false ;
vtDir1 /= dLen1 ;
Vector3d vtN1( vtDir1.y, - vtDir1.x, 0) ;
// linea per successivi controlli
CurveLine NewLine1 ;
// se punto a minima distanza da C non interno al segmento, eseguo direttamente culling su intera linea
double dUMin1 = ( ptCen - ptS1) * vtDir1 ;
if ( dUMin1 < EPS_SMALL || dUMin1 > dLen1 - EPS_SMALL) {
Vector3d vtRad = 0.5 * ( ptE1 + ptS1) - ptCen ;
Vector3d vtMove = ( bCCW ? Vector3d( - vtRad.y, vtRad.x, 0) : Vector3d( vtRad.y, - vtRad.x, 0)) ;
if ( vtN1 * vtMove <= 0)
return true ;
}
// altrimenti tengo parte con culling superato
else {
// verifico primo estremo
Vector3d vtRad = ptS1 - ptCen ;
Vector3d vtMove = ( bCCW ? Vector3d( - vtRad.y, vtRad.x, 0) : Vector3d( vtRad.y, - vtRad.x, 0)) ;
// se va bene il primo estremo
if ( vtN1 * vtMove > 0) {
ptE1 = ptS1 + vtDir1 * dUMin1 ;
dLen1 = dUMin1 ;
}
// altrimenti deve andare bene il secondo
else {
ptS1 += vtDir1 * dUMin1 ;
dLen1 -= dUMin1 ;
}
NewLine1.Set( ptE1, ptS1) ;
pLine1 = &NewLine1 ;
}
// analisi della seconda linea
Point3d ptS2 = pLine2->GetStart() ;
Point3d ptE2 = pLine2->GetEnd() ;
Vector3d vtDir2 = ptE2 - ptS2 ;
vtDir2.z = 0 ;
double dLen2 = vtDir2.LenXY() ;
if ( dLen2 < EPS_SMALL)
return false ;
vtDir2 /= dLen2 ;
Vector3d vtN2( vtDir2.y, - vtDir2.x, 0) ;
// linea per successivi controlli
CurveLine NewLine2 ;
// se punto a minima distanza da C non interno al segmento, eseguo direttamente culling su intera linea
double dUMin2 = ( ptCen - ptS2) * vtDir2 ;
if ( dUMin2 < EPS_SMALL || dUMin2 > dLen2 - EPS_SMALL) {
Vector3d vtRad = 0.5 * ( ptE2 + ptS2) - ptCen ;
Vector3d vtMove = ( bCCW ? Vector3d( - vtRad.y, vtRad.x, 0) : Vector3d( vtRad.y, - vtRad.x, 0)) ;
if ( vtN2 * vtMove >= 0)
return true ;
}
// altrimenti tengo parte con culling superato
else {
// verifico primo estremo
Vector3d vtRad = ptS2 - ptCen ;
Vector3d vtMove = ( bCCW ? Vector3d( - vtRad.y, vtRad.x, 0) : Vector3d( vtRad.y, - vtRad.x, 0)) ;
// se va bene il primo estremo
if ( vtN2 * vtMove < 0) {
ptE2 = ptS2 + vtDir2 * dUMin2 ;
dLen2 = dUMin2 ;
}
// altrimenti deve andare bene il secondo
else {
ptS2 += vtDir2 * dUMin2 ;
dLen2 -= dUMin2 ;
}
NewLine2.Set( ptE2, ptS2) ;
pLine2 = &NewLine2 ;
}
// verifico se gli intervalli radiali si sovrappongono
Vector3d vtDirS1 = ptS1 - ptCen ;
vtDirS1.z = 0 ;
double dRadS1 = vtDirS1.LenXY() ;
Vector3d vtDirE1 = ptE1 - ptCen ;
vtDirE1.z = 0 ;
double dRadE1 = vtDirE1.LenXY() ;
Vector3d vtDirS2 = ptS2 - ptCen ;
vtDirS2.z = 0 ;
double dRadS2 = vtDirS2.LenXY() ;
Vector3d vtDirE2 = ptE2 - ptCen ;
vtDirE2.z = 0 ;
double dRadE2 = vtDirE2.LenXY() ;
// ordino l'intervallo della prima linea secondo raggi crescenti
if ( dRadS1 > dRadE1) {
swap( ptS1, ptE1) ;
swap( vtDirS1, vtDirE1) ;
swap( dRadS1, dRadE1) ;
}
// ordino l'intervallo della seconda linea secondo raggi crescenti
if ( dRadS2 > dRadE2) {
swap( ptS2, ptE2) ;
swap( vtDirS2, vtDirE2) ;
swap( dRadS2, dRadE2) ;
}
// se gli intervalli non si sovrappongono, non ci sono limiti
if ( dRadS1 > dRadE2 - EPS_SMALL || dRadS2 > dRadE1 - EPS_SMALL)
return true ;
// calcolo la minima rotazione degli estremi interni all'intervallo comune
double dNewAng = dAng ;
if ( ! AreSamePointXYEpsilon( ptS1, ptS2, 10 * EPS_SMALL)) {
if ( dRadS1 >= dRadS2) {
vtDirS1 /= dRadS1 ;
CurveArc ArcS1 ;
ArcS1.Set( ptCen, Z_AX, dRadS1, vtDirS1, dAng, 0) ;
IntersLineArc intLA( *pLine2, ArcS1) ;
if ( intLA.GetNumInters() == 2) {
IntCrvCrvInfo iccInfo1, iccInfo2 ;
intLA.GetIntCrvCrvInfo( 0, iccInfo1) ;
intLA.GetIntCrvCrvInfo( 1, iccInfo2) ;
dNewAng = dAng * min( iccInfo1.IciB[0].dU, iccInfo2.IciB[0].dU) ;
}
else if ( intLA.GetNumInters() == 1) {
IntCrvCrvInfo iccInfo ;
intLA.GetIntCrvCrvInfo( 0, iccInfo) ;
dNewAng = dAng * iccInfo.IciB[0].dU ;
}
}
else {
vtDirS2 /= dRadS2 ;
CurveArc ArcS2 ;
ArcS2.Set( ptCen, Z_AX, dRadS2, vtDirS2, - dAng, 0) ;
IntersLineArc intLA( *pLine1, ArcS2) ;
if ( intLA.GetNumInters() == 2) {
IntCrvCrvInfo iccInfo1, iccInfo2 ;
intLA.GetIntCrvCrvInfo( 0, iccInfo1) ;
intLA.GetIntCrvCrvInfo( 1, iccInfo2) ;
dNewAng = dAng * min( iccInfo1.IciB[0].dU, iccInfo2.IciB[0].dU) ;
}
else if ( intLA.GetNumInters() == 1) {
IntCrvCrvInfo iccInfo ;
intLA.GetIntCrvCrvInfo( 0, iccInfo) ;
dNewAng = dAng * iccInfo.IciB[0].dU ;
}
}
}
if ( ! AreSamePointXYEpsilon( ptE1, ptE2, 10 * EPS_SMALL)) {
if ( dRadE1 <= dRadE2) {
vtDirE1 /= dRadE1 ;
CurveArc ArcE1 ;
ArcE1.Set( ptCen, Z_AX, dRadE1, vtDirE1, dAng, 0) ;
IntersLineArc intLA( *pLine2, ArcE1) ;
if ( intLA.GetNumInters() == 2) {
IntCrvCrvInfo iccInfo1, iccInfo2 ;
intLA.GetIntCrvCrvInfo( 0, iccInfo1) ;
intLA.GetIntCrvCrvInfo( 1, iccInfo2) ;
if ( bCCW)
dNewAng = min( dNewAng, dAng * min( iccInfo1.IciB[0].dU, iccInfo2.IciB[0].dU)) ;
else
dNewAng = max( dNewAng, dAng * min( iccInfo1.IciB[0].dU, iccInfo2.IciB[0].dU)) ;
}
else if ( intLA.GetNumInters() == 1) {
IntCrvCrvInfo iccInfo ;
intLA.GetIntCrvCrvInfo( 0, iccInfo) ;
if ( bCCW)
dNewAng = min( dNewAng, dAng * iccInfo.IciB[0].dU) ;
else
dNewAng = max( dNewAng, dAng * iccInfo.IciB[0].dU) ;
}
}
else {
vtDirE2 /= dRadE2 ;
CurveArc ArcE2 ;
ArcE2.Set( ptCen, Z_AX, dRadE2, vtDirE2, - dAng, 0) ;
IntersLineArc intLA( *pLine1, ArcE2) ;
if ( intLA.GetNumInters() == 2) {
IntCrvCrvInfo iccInfo1, iccInfo2 ;
intLA.GetIntCrvCrvInfo( 0, iccInfo1) ;
intLA.GetIntCrvCrvInfo( 1, iccInfo2) ;
if ( bCCW)
dNewAng = min( dNewAng, dAng * min( iccInfo1.IciB[0].dU, iccInfo2.IciB[0].dU)) ;
else
dNewAng = max( dNewAng, dAng * min( iccInfo1.IciB[0].dU, iccInfo2.IciB[0].dU)) ;
}
else if ( intLA.GetNumInters() == 1) {
IntCrvCrvInfo iccInfo ;
intLA.GetIntCrvCrvInfo( 0, iccInfo) ;
if ( bCCW)
dNewAng = min( dNewAng, dAng * iccInfo.IciB[0].dU) ;
else
dNewAng = max( dNewAng, dAng * iccInfo.IciB[0].dU) ;
}
}
}
if ( AreSamePointXYEpsilon( ptS1, ptS2, 10 * EPS_SMALL) &&
AreSamePointXYEpsilon( ptE1, ptE2, 10 * EPS_SMALL))
dNewAng = 0 ;
// confronto con rotazione corrente
if ( abs( dNewAng) < EPS_ANG_SMALL)
dAng = 0 ;
else if ( bCCW && dNewAng > 0)
dAng = min( dAng, dNewAng) ;
else if ( ! bCCW && dNewAng < 0)
dAng = max( dAng, dNewAng) ;
return true ;
}
Reference in New Issue View Git Blame Copy Permalink