Dario Sassi
50f653750b
- aggiunte funzioni per verifica di collisione tra Tronco di Cono e Triangolo e tra Tronco di Piramide Generalizzato e Triangolo - migliorate funzioni per verifica di collisione tra Tronco di Cono e Zmap e tra Tronco di Piramide Generalizzato e Zmap.
228 lines
8.1 KiB
C++
228 lines
8.1 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2020-2020
|
|
//----------------------------------------------------------------------------
|
|
// File : DistLineLine.h Data : 06.11.20 Versione : 2.2k1
|
|
// Contenuto : Implementazione della classe distanza fra elementi lineari.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 06.11.20 LM Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
#include "stdafx.h"
|
|
#include "DistLineLine.h"
|
|
#include "/EgtDev/Include/EgtNumUtils.h"
|
|
#include "/EgtDev/Include/EGkGeoCollection.h"
|
|
#include "/EgtDev/Include/EGkGeoConst.h"
|
|
#include <algorithm>
|
|
|
|
//----------------------------------------------------------------------------
|
|
DistLineLine::DistLineLine( const Point3d& ptSt1, const Point3d& ptEn1,
|
|
const Point3d& ptSt2, const Point3d& ptEn2,
|
|
bool bIsSegment1, bool bIsSegment2)
|
|
{
|
|
Vector3d vtD1 = ptEn1 - ptSt1 ;
|
|
double dLen1 = vtD1.Len() ;
|
|
Vector3d vtD2 = ptEn2 - ptSt2 ;
|
|
double dLen2 = vtD1.Len() ;
|
|
if ( dLen1 < EPS_SMALL || dLen2 < EPS_SMALL) {
|
|
m_dSqDist = - 1 ;
|
|
m_dDist = - 1 ;
|
|
return ;
|
|
}
|
|
vtD1 /= dLen1 ;
|
|
vtD2 /= dLen2 ;
|
|
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)
|
|
{
|
|
if ( dLen1 < EPS_SMALL || dLen2 < EPS_SMALL) {
|
|
m_dSqDist = - 1 ;
|
|
m_dDist = - 1 ;
|
|
return ;
|
|
}
|
|
Calculate( ptSt1, vtD1, dLen1, ptSt2, vtD2, dLen2, bIsSegment1, bIsSegment2) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
DistLineLine::GetSqDist( double& dSqDist)
|
|
{
|
|
if ( m_dSqDist < 0)
|
|
return false ;
|
|
dSqDist = m_dSqDist ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
DistLineLine::GetDist( double& dDist)
|
|
{
|
|
if ( m_dSqDist < 0)
|
|
return false ;
|
|
dDist = sqrt( m_dSqDist) ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
DistLineLine::GetMinDistPoints( Point3d& ptMinDist1, Point3d& ptMinDist2)
|
|
{
|
|
if ( m_dSqDist < 0)
|
|
return false ;
|
|
ptMinDist1 = m_ptMinDist1 ;
|
|
ptMinDist2 = m_ptMinDist2 ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
DistLineLine::GetParamsAtMinDistPoints( double& dPar1, double& dPar2)
|
|
{
|
|
if ( m_dSqDist < 0)
|
|
return false ;
|
|
dPar1 = m_dPar1 ;
|
|
dPar2 = m_dPar2 ;
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Calcola la distanza fra i due elemnti lineari, i punti di minima distanza e
|
|
// i loro rispettivi parametri.
|
|
// Se la coppia di punti di minima distanza non è unica ne viene scelta una
|
|
// in base a comodità di calcolo.
|
|
void
|
|
DistLineLine::Calculate( const Point3d& ptSt1, const Vector3d& vtD1, double dLen1,
|
|
const Point3d& ptSt2, const Vector3d& vtD2, double dLen2,
|
|
bool bIsSegment1, bool bIsSegment2)
|
|
{
|
|
// Caso di elementi lineari paralleli/antiparalleli
|
|
if ( AreSameOrOppositeVectorExact( vtD1, vtD2)) {
|
|
// Almeno un elemento è una retta
|
|
if ( ! ( bIsSegment1 && bIsSegment2)) {
|
|
// Il primo elemento è segmento, quindi deve essere una retta il secondo
|
|
if ( bIsSegment1) {
|
|
Vector3d vtStSt = ptSt1 - ptSt2 ;
|
|
double dLong = vtStSt * vtD2 ;
|
|
Vector3d vtDist = vtStSt - dLong * vtD2 ;
|
|
m_dSqDist = vtDist.SqLen() ;
|
|
m_dDist = sqrt( m_dSqDist) ;
|
|
m_dPar1 = 0 ;
|
|
m_dPar2 = dLong ;
|
|
m_ptMinDist1 = ptSt1 ;
|
|
m_ptMinDist2 = ptSt2 + dLong * vtD2 ;
|
|
}
|
|
// Il primo elemento è una retta
|
|
else {
|
|
Vector3d vtStSt = ptSt2 - ptSt1 ;
|
|
double dLong = vtStSt * vtD1 ;
|
|
Vector3d vtDist = vtStSt - dLong * vtD1 ;
|
|
m_dSqDist = vtDist.SqLen() ;
|
|
m_dDist = sqrt( m_dSqDist) ;
|
|
m_dPar1 = dLong ;
|
|
m_dPar2 = 0 ;
|
|
m_ptMinDist1 = ptSt1 + dLong * vtD1 ;
|
|
m_ptMinDist2 = ptSt2 ;
|
|
}
|
|
}
|
|
// 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 segmento 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_dDist = sqrt( m_dSqDist) ;
|
|
m_ptMinDist1 = ptEn1 ;
|
|
m_ptMinDist2 = ptMinPar ;
|
|
m_dPar1 = dLen1 ;
|
|
m_dPar2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
|
|
}
|
|
else if ( dMinPar > 0) {
|
|
m_dSqDist = std::max( vtStSt * vtStSt - dStU * dStU, 0.) ;
|
|
m_dDist = sqrt( m_dSqDist) ;
|
|
m_ptMinDist1 = ptSt1 + dMinPar * vtD1 ;
|
|
m_ptMinDist2 = ptMinPar ;
|
|
m_dPar1 = dMinPar ;
|
|
m_dPar2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
|
|
}
|
|
else if ( dMaxPar > 0) {
|
|
m_dSqDist = std::max( vtStSt * vtStSt - dStU * dStU, 0.) ;
|
|
m_dDist = sqrt( m_dSqDist) ;
|
|
m_ptMinDist1 = ptSt1 ;
|
|
m_ptMinDist2 = ptSt2 + ( ptSt1 - ptSt2) * vtD2 * vtD2 ;
|
|
m_dPar1 = 0 ;
|
|
m_dPar2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
|
|
}
|
|
else {
|
|
m_dSqDist = SqDist( ptSt1, ptMaxPar) ;
|
|
m_dDist = sqrt( m_dSqDist) ;
|
|
m_ptMinDist1 = ptSt1 ;
|
|
m_ptMinDist2 = ptMaxPar ;
|
|
m_dPar1 = 0 ;
|
|
m_dPar2 = Clamp( ( m_ptMinDist2 - ptSt2) * vtD2, 0., dLen2) ;
|
|
}
|
|
}
|
|
return ;
|
|
}
|
|
// Caso generale
|
|
Vector3d vtDist0 = ptSt2 - ptSt1 ;
|
|
double dDist01 = vtDist0 * vtD1 ;
|
|
double dDist02 = vtDist0 * vtD2 ;
|
|
double dDotD1D2 = vtD1 * vtD2 ;
|
|
double dT1 = dDist01 + ( ( dDist01 * dDotD1D2 - dDist02) * dDotD1D2) / ( 1 - dDotD1D2 * dDotD1D2) ;
|
|
double dT2 = ( dDist01 * dDotD1D2 - dDist02) / ( 1 - dDotD1D2 * dDotD1D2) ;
|
|
double dMin1 = - INFINITO ;
|
|
double dMax1 = INFINITO ;
|
|
double dMin2 = - INFINITO ;
|
|
double dMax2 = INFINITO ;
|
|
double dSt1On2 = ( ptSt1 - ptSt2) * vtD2 ;
|
|
double dEn1On2 = ( ptSt1 + dLen1 * vtD1 - ptSt2) * vtD2 ;
|
|
if ( bIsSegment1) {
|
|
dMin1 = 0 ;
|
|
dMax1 = dLen1 ;
|
|
dMin2 = std::min( dSt1On2, dEn1On2) ;
|
|
dMax2 = std::max( dSt1On2, dEn1On2) ;
|
|
}
|
|
if ( bIsSegment2) {
|
|
double dSt2On1 = ( ptSt2 - ptSt1) * vtD1 ;
|
|
double dEn2On1 = ( ptSt2 + dLen2 * vtD2 - ptSt1) * vtD1 ;
|
|
dMin1 = std::max( dMin1, std::min( dSt2On1, dEn2On1)) ;
|
|
dMax1 = std::min( dMax1, std::max( dSt2On1, dEn2On1)) ;
|
|
dMin2 = std::max( dMin2, 0.) ;
|
|
dMax2 = std::min( dMax2, dLen2) ;
|
|
}
|
|
m_dPar1 = Clamp( dT1, dMin1, dMax1) ;
|
|
m_dPar2 = Clamp( dT2, dMin2, dMax2) ;
|
|
m_ptMinDist1 = ptSt1 + m_dPar1 * vtD1 ;
|
|
m_ptMinDist2 = ptSt2 + m_dPar2 * vtD2 ;
|
|
m_dSqDist = SqDist( m_ptMinDist1, m_ptMinDist2) ;
|
|
m_dDist = sqrt( m_dSqDist) ;
|
|
}
|