Dario Sassi
17759877d4
- interfaccia DistLineLine ora esportata - in lettura Curve Composite allargata tolleranza giunzione tra componenti.
232 lines
8.4 KiB
C++
232 lines
8.4 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2020-2024
|
|
//----------------------------------------------------------------------------
|
|
// File : DistLineLine.cpp Data : 10.05.24 Versione : 2.6e3
|
|
// Contenuto : Implementazione della classe distanza fra elementi lineari.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 06.11.20 LM Creazione modulo.
|
|
// 12.08.22 DS Correzioni e migliorie varie.
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
#include "stdafx.h"
|
|
#include "/EgtDev/Include/EGkDistLineLine.h"
|
|
#include "/EgtDev/Include/EgtNumUtils.h"
|
|
#include "/EgtDev/Include/EGkGeoCollection.h"
|
|
#include "/EgtDev/Include/EGkGeoConst.h"
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
DistLineLine::DistLineLine( const Point3d& ptSt1, const Point3d& ptEn1,
|
|
const Point3d& ptSt2, const Point3d& ptEn2,
|
|
bool bIsSegment1, bool bIsSegment2)
|
|
{
|
|
// reset oggetto
|
|
m_dSqDist = - 1 ;
|
|
m_dDist = - 1 ;
|
|
// calcolo direzione segmenti
|
|
Vector3d vtD1 = ptEn1 - ptSt1 ;
|
|
double dLen1 = vtD1.Len() ;
|
|
Vector3d vtD2 = ptEn2 - ptSt2 ;
|
|
double dLen2 = vtD2.Len() ;
|
|
if ( dLen1 < EPS_SMALL || dLen2 < EPS_SMALL)
|
|
return ;
|
|
vtD1 /= dLen1 ;
|
|
vtD2 /= dLen2 ;
|
|
// eseguo
|
|
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)
|
|
{
|
|
// reset oggetto
|
|
m_dSqDist = - 1 ;
|
|
m_dDist = - 1 ;
|
|
// verifico segmenti
|
|
if ( dLen1 < EPS_SMALL || dLen2 < EPS_SMALL)
|
|
return ;
|
|
// eseguo
|
|
Calculate( ptSt1, vtD1, dLen1, ptSt2, vtD2, dLen2, bIsSegment1, bIsSegment2) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
DistLineLine::GetSqDist( double& dSqDist) const
|
|
{
|
|
if ( m_dSqDist < 0)
|
|
return false ;
|
|
|
|
dSqDist = m_dSqDist ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
DistLineLine::GetDist( double& dDist) const
|
|
{
|
|
if ( m_dSqDist < 0)
|
|
return false ;
|
|
|
|
if ( m_dDist < 0)
|
|
m_dDist = sqrt( m_dSqDist) ;
|
|
dDist = m_dDist ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
DistLineLine::GetMinDistPoints( Point3d& ptMinDist1, Point3d& ptMinDist2) const
|
|
{
|
|
if ( m_dSqDist < 0)
|
|
return false ;
|
|
ptMinDist1 = m_ptMinDist1 ;
|
|
ptMinDist2 = m_ptMinDist2 ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
DistLineLine::GetPositionsAtMinDistPoints( double& dPos1, double& dPos2) const
|
|
{
|
|
if ( m_dSqDist < 0)
|
|
return false ;
|
|
dPos1 = m_dPos1 ;
|
|
dPos2 = m_dPos2 ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Calcola la distanza fra i due elementi lineari, i punti di minima distanza e le loro posizioni.
|
|
// Se i due elementi sono paralleli i punti di minimo sono scelti secondo convenienza.
|
|
void
|
|
DistLineLine::Calculate( const Point3d& ptSt1, const Vector3d& vtD1, double dLen1,
|
|
const Point3d& ptSt2, const Vector3d& vtD2, double dLen2,
|
|
bool bIsSegment1, bool bIsSegment2)
|
|
{
|
|
// Se elementi paralleli o antiparalleli
|
|
if ( AreSameOrOppositeVectorExact( vtD1, vtD2)) {
|
|
// Se il primo elemento è una retta infinita
|
|
if ( ! bIsSegment1) {
|
|
Vector3d vtStSt = ptSt2 - ptSt1 ;
|
|
double dLong = vtStSt * vtD1 ;
|
|
Vector3d vtDist = vtStSt - dLong * vtD1 ;
|
|
m_dSqDist = vtDist.SqLen() ;
|
|
m_dPos1 = dLong ;
|
|
m_dPos2 = 0 ;
|
|
m_ptMinDist1 = ptSt1 + dLong * vtD1 ;
|
|
m_ptMinDist2 = ptSt2 ;
|
|
}
|
|
// se altrimenti il secondo elemento è una retta infinita
|
|
else if ( ! bIsSegment2) {
|
|
Vector3d vtStSt = ptSt1 - ptSt2 ;
|
|
double dLong = vtStSt * vtD2 ;
|
|
Vector3d vtDist = vtStSt - dLong * vtD2 ;
|
|
m_dSqDist = vtDist.SqLen() ;
|
|
m_dPos1 = 0 ;
|
|
m_dPos2 = dLong ;
|
|
m_ptMinDist1 = ptSt1 ;
|
|
m_ptMinDist2 = ptSt2 + dLong * vtD2 ;
|
|
}
|
|
// altrimenti 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 secondo 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_ptMinDist1 = ptEn1 ;
|
|
m_ptMinDist2 = ptMinPar ;
|
|
m_dPos1 = dLen1 ;
|
|
m_dPos2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
|
|
}
|
|
else if ( dMinPar > 0) {
|
|
m_dSqDist = max( vtStSt * vtStSt - dStU * dStU, 0.) ;
|
|
m_ptMinDist1 = ptSt1 + dMinPar * vtD1 ;
|
|
m_ptMinDist2 = ptMinPar ;
|
|
m_dPos1 = dMinPar ;
|
|
m_dPos2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
|
|
}
|
|
else if ( dMaxPar > 0) {
|
|
m_dSqDist = max( vtStSt * vtStSt - dStU * dStU, 0.) ;
|
|
m_ptMinDist1 = ptSt1 ;
|
|
m_ptMinDist2 = ptSt2 + ( ptSt1 - ptSt2) * vtD2 * vtD2 ;
|
|
m_dPos1 = 0 ;
|
|
m_dPos2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
|
|
}
|
|
else {
|
|
m_dSqDist = SqDist( ptSt1, ptMaxPar) ;
|
|
m_ptMinDist1 = ptSt1 ;
|
|
m_ptMinDist2 = ptMaxPar ;
|
|
m_dPos1 = 0 ;
|
|
m_dPos2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
|
|
}
|
|
}
|
|
}
|
|
// Caso generale
|
|
else {
|
|
// Posizioni a distanza minima tra rette illimitate
|
|
Vector3d vtStSt = ptSt2 - ptSt1 ;
|
|
double dDist01 = vtStSt * vtD1 ;
|
|
double dDist02 = vtStSt * vtD2 ;
|
|
double dDotD1D2 = vtD1 * vtD2 ;
|
|
double dT1 = ( dDist01 - dDotD1D2 * dDist02) / ( 1 - dDotD1D2 * dDotD1D2) ;
|
|
double dT2 = ( dDotD1D2 * dDist01 - dDist02) / ( 1 - dDotD1D2 * dDotD1D2) ;
|
|
// Posizioni minime e massime sui segmenti
|
|
double dMin1 = ( bIsSegment1 ? 0 : -INFINITO) ;
|
|
double dMax1 = ( bIsSegment1 ? dLen1 : INFINITO) ;
|
|
double dMin2 = ( bIsSegment2 ? 0 : -INFINITO) ;
|
|
double dMax2 = ( bIsSegment2 ? dLen2 : INFINITO) ;
|
|
// Se entrambe le posizioni stanno nei segmenti
|
|
if ( dT1 >= dMin1 && dT1 <= dMax1 && dT2 >= dMin2 && dT2 <= dMax2) {
|
|
m_dPos1 = dT1 ;
|
|
m_dPos2 = dT2 ;
|
|
}
|
|
// se altrimenti solo la prima sta nel segmento
|
|
else if ( dT1 >= dMin1 && dT1 <= dMax1) {
|
|
m_dPos2 = Clamp( dT2, dMin2, dMax2) ;
|
|
m_dPos1 = Clamp( (( ptSt2 + m_dPos2 * vtD2) - ptSt1) * vtD1, dMin1, dMax1) ;
|
|
}
|
|
// se altrimenti solo la seconda sta nel segmento
|
|
else if ( dT2 >= dMin2 && dT2 <= dMax2) {
|
|
m_dPos1 = Clamp( dT1, dMin1, dMax1) ;
|
|
m_dPos2 = Clamp( (( ptSt1 + m_dPos1 * vtD1) - ptSt2) * vtD2, dMin2, dMax2) ;
|
|
}
|
|
// altrimenti nessuna sta nel suo segmento
|
|
else {
|
|
m_dPos1 = Clamp( dT1, dMin1, dMax1) ;
|
|
m_dPos2 = Clamp( (( ptSt1 + m_dPos1 * vtD1) - ptSt2) * vtD2, dMin2, dMax2) ;
|
|
m_dPos1 = Clamp( (( ptSt2 + m_dPos2 * vtD2) - ptSt1) * vtD1, dMin1, dMax1) ;
|
|
}
|
|
m_ptMinDist1 = ptSt1 + m_dPos1 * vtD1 ;
|
|
m_ptMinDist2 = ptSt2 + m_dPos2 * vtD2 ;
|
|
m_dSqDist = SqDist( m_ptMinDist1, m_ptMinDist2) ;
|
|
}
|
|
}
|