Dario Sassi
372 lines
16 KiB
C++
372 lines
16 KiB
C++
#include "stdafx.h"
|
|
#include "CDeUtility.h"
|
|
#include "/EgtDev/Include/EGkDistPointLine.h"
|
|
#include "/EgtDev/Include/ENkPolynomialRoots.h"
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Dati un punto e una retta, restituisce il quadrato della loro distanza.
|
|
// La retta è descritta da un suo punto e dal suo versore tangente.
|
|
double
|
|
GetPointLineSqDist( const Point3d& ptP, const Point3d& ptLine, const Vector3d& vtLine)
|
|
{
|
|
return ( ( ptP - ptLine) - ( ptP - ptLine) * vtLine * vtLine).SqLen() ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Dati un punto e un piano, ne restituisce la distanza con segno.
|
|
// Il piano è descritto da un suo punto e dal suo versore normale.
|
|
double
|
|
PointPlaneSignedDist( const Point3d& ptP, const Point3d& ptPlane, const Vector3d& vtNorm)
|
|
{
|
|
return ( ptP - ptPlane) * vtNorm ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Dati un sistema di tre punti e un piano, ne calcola le tre distanze con segno e ne restituisce la
|
|
// maggiore. I tre punti sono passati mediante un triangolo, il piano è descritto da un suo punto e
|
|
// dal suo versore normale.
|
|
double
|
|
ThreePointPlaneSignedDist( const Triangle3d& trTria, const Point3d& ptPlane, const Vector3d& vtNorm)
|
|
{
|
|
double dMaxSgnDist = - INFINITO ;
|
|
for ( int nV = 0 ; nV < 3 ; ++ nV) {
|
|
double dCurrSgnDist = PointPlaneSignedDist( trTria.GetP( nV), ptPlane, vtNorm) ;
|
|
if ( dCurrSgnDist > dMaxSgnDist)
|
|
dMaxSgnDist = dCurrSgnDist ;
|
|
}
|
|
return dMaxSgnDist ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Calcola, se esiste, il punto di una retta a distanza con segno data da un piano.
|
|
// Il piano è descritto da punto e versore normale.
|
|
// La retta è descritta da punto e versore direzione.
|
|
// I casi possibili sono:
|
|
// - Nessun punto della retta dista d dal piano (retta parallela al piano con distanza diversa da d),
|
|
// viene restituito 0 e il valore di dPar non ha senso.
|
|
// - Un solo punto della retta dista d dal piano (retta non parallela al piano), viene restituito 1
|
|
// e il valore di dPar è il parametro del punto sulla retta.
|
|
// - Tutti i punti della retta distano d dal piano (retta parallela al piano con distanza d), viene
|
|
// restituito 2 e qualunque valore di dPar ha senso.
|
|
int
|
|
LinePlaneDDistPar( const Point3d& ptPlane, const Vector3d& vtPlane, const Point3d& ptLine, const Vector3d& vtLine,
|
|
double dDist, double& dPar)
|
|
{
|
|
// Vettore congiungente il punto del piano con il punto iniziale della retta
|
|
Vector3d vtPlLn = ptLine - ptPlane ;
|
|
// Distanza con segno del punto iniziale della retta dal piano
|
|
double dSgnDist0 = vtPlLn * vtPlane ;
|
|
// Prodotto scalare fra versore della direzione della retta e versore normale al piano
|
|
double dDotDirNorm = vtLine * vtPlane ;
|
|
// Retta parallela al piano
|
|
if ( abs( dDotDirNorm) < EPS_ZERO) {
|
|
if ( abs( dSgnDist0 - dDist) < EPS_SMALL)
|
|
return 2 ;
|
|
else
|
|
return 0 ;
|
|
}
|
|
// Caso generale
|
|
else {
|
|
dPar = ( dDist - dSgnDist0) / dDotDirNorm ;
|
|
return 1 ;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Date due rette nello spazio restituisce il quadrato della loro distanza.
|
|
// Le rette sono definite da un punto e dal versore direzione.
|
|
double
|
|
LineLineSqDist( const Point3d& ptP1, const Vector3d& vtD1, const Point3d& ptP2, const Vector3d& vtD2)
|
|
{
|
|
// Se parallele
|
|
if ( abs( 1. - abs( vtD1 * vtD2)) < EPS_SMALL)
|
|
return ( ( ptP2 - ptP1) - ( ptP2 - ptP1) * vtD1 * vtD1).SqLen() ;
|
|
// Caso generale
|
|
else {
|
|
Vector3d vtOrtD2 = vtD2 - vtD2 * vtD1 * vtD1 ;
|
|
Vector3d vtOrtP1P2 = ( ptP2 - ptP1) - (ptP2 - ptP1) * vtD1 * vtD1 ;
|
|
double dD = ( vtOrtD2 * vtOrtP1P2) / vtOrtP1P2.SqLen() ;
|
|
return ( vtOrtP1P2.SqLen() + dD * dD * vtOrtD2.SqLen() + 2 * dD * vtOrtD2 * vtOrtP1P2) ;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Data una retta e un segmento, ne restituisce il quadrato della distanza.
|
|
// La retta è definita da un punto e dal versore direzione.
|
|
// Il segmento è definito da un estremo, dal versore direzione verso l'altro estremo e dalla lunghezza.
|
|
double
|
|
LineSegmentSqDist( const Point3d& ptPLn, const Vector3d& vtDLn,
|
|
const Point3d& ptPSg, const Vector3d& vtDSg, double dSgLen)
|
|
{
|
|
// Se paralleli
|
|
if ( AreSameOrOppositeVectorApprox( vtDLn, vtDSg))
|
|
return ( ( ptPSg - ptPLn) - ( ptPSg - ptPLn) * vtDLn * vtDLn).SqLen() ;
|
|
|
|
// Caso generale
|
|
else {
|
|
double dDLn = ( ptPSg - ptPLn) * vtDLn ;
|
|
double dDSg = ( ptPSg - ptPLn) * vtDSg ;
|
|
double dDLnSg = vtDLn * vtDSg ;
|
|
// posizione parametrica del punto sul segmento
|
|
double dSgPar = ( dDLn * dDLnSg - dDSg) / ( 1. - dDLnSg * dDLnSg) ;
|
|
// se prima dell'inizio, è distanza dell'inizio dalla linea
|
|
if ( dSgPar <= 0.)
|
|
return GetPointLineSqDist( ptPSg, ptPLn, vtDLn) ;
|
|
// se dopo fine, è distanza fine dalla linea
|
|
else if ( dSgPar >= dSgLen)
|
|
return GetPointLineSqDist( ptPSg + dSgLen * vtDSg, ptPLn, vtDLn) ;
|
|
// altrimenti, determino i due punti a minima distanza
|
|
else {
|
|
double dLnPar = ( dDLn - dDSg * dDLnSg) / ( 1. - dDLnSg * dDLnSg) ;
|
|
Point3d ptMinLn = ptPLn + dLnPar * vtDLn ;
|
|
Point3d ptMinSg = ptPSg + dSgPar * vtDSg ;
|
|
return SqDist( ptMinLn, ptMinSg) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Valuta l'interferenza di una circonferenza (disco vuoto) e un triangolo (pieno) complanari.
|
|
// Si assume che disco e piano siano paralleli, ovvero che valga a n = 1 con
|
|
// a versore dell'asse del disco e n versore normale al triangolo.
|
|
// L'informazione su n è contenuta nell'oggetto triangolo; quella su a non è passata
|
|
// in quanto inutile sotto le nostre assunzioni.
|
|
bool
|
|
CoplanarCircleTriangleInterference( const Point3d& ptCen, double dRad, const Triangle3d& trTria)
|
|
{
|
|
// Se il centro del disco non sta sul piano del triangolo, non sono complanari
|
|
if ( abs( PointPlaneSignedDist( ptCen, trTria.GetP( 0), trTria.GetN())) > EPS_SMALL)
|
|
return false ;
|
|
// Classifico i vertici del triangolo in base alla loro posizione rispetto alla circonferenza.
|
|
double dSqRad = dRad * dRad ;
|
|
double dSqRadInf = dSqRad - 2 * dRad * EPS_SMALL ;
|
|
double dSqRadSup = dSqRad + 2 * dRad * EPS_SMALL ;
|
|
int nInOutVec[3] ;
|
|
for ( int n = 0 ; n < 3 ; ++ n) {
|
|
Point3d ptTriaVert = trTria.GetP( n) ;
|
|
nInOutVec[n] = SqDist( ptTriaVert, ptCen) < dSqRadInf ? 1 : SqDist( ptTriaVert, ptCen) < dSqRadSup ? 0 : - 1 ;
|
|
}
|
|
// Se i vertici sono tutti dentro, non c'è interferenza.
|
|
if ( nInOutVec[0] == 1 && nInOutVec[1] == 1 && nInOutVec[2] == 1)
|
|
return false ;
|
|
// Se i vertici sono tutti fuori, valuto se il centro della circonferenza è interno al triangolo.
|
|
else if ( nInOutVec[0] == - 1 && nInOutVec[1] == - 1 && nInOutVec[2] == - 1) {
|
|
return IsPointInsideTriangle( ptCen, trTria, TriangleType::EXACT) ;
|
|
}
|
|
// Almeno un vertice tocca la circonferenza oppure due segmenti la attraversano, quindi c'è interferenza.
|
|
else
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Valuta l'interferenza di un disco e un triangolo complanari.
|
|
// Si assume che disco e piano siano paralleli, ovvero che valga a n = 1 con
|
|
// a versore dell'asse del disco e n versore normale al triangolo.
|
|
// L'informazione su n è contenuta nell'oggetto triangolo; quella su a non è passata
|
|
// in quanto inutile sotto le nostre assunzioni.
|
|
bool
|
|
CoplanarDiscTriangleInterference( const Point3d& ptCen, double dRad, const Triangle3d& trTria, int nTriType)
|
|
{
|
|
// Se il centro del disco non sta sul piano del triangolo, non sono complanari
|
|
if ( abs( PointPlaneSignedDist( ptCen, trTria.GetP( 0), trTria.GetN())) > EPS_SMALL)
|
|
return false ;
|
|
// Se il centro giace nel triangolo
|
|
if ( IsPointInsideTriangle( ptCen, trTria, TriangleType::EXACT))
|
|
return true ;
|
|
|
|
double dSqMinDist = 0 ;
|
|
switch ( nTriType) {
|
|
case OPEN:
|
|
dSqMinDist = dRad * dRad - 2 * dRad * EPS_SMALL ;
|
|
break ;
|
|
case EXACT:
|
|
dSqMinDist = dRad * dRad ;
|
|
break ;
|
|
case CLOSED:
|
|
dSqMinDist = dRad * dRad + 2 * dRad * EPS_SMALL ;
|
|
break ;
|
|
}
|
|
|
|
// Se il centro non si avvicina più del raggio dalle rette associate ai segmenti, non c'è interferenza
|
|
double dSegPointDist0, dSegPointDist1, dSegPointDist2 ;
|
|
( DistPointLine( ptCen, trTria.GetP( 0), trTria.GetP( 1))).GetSqDist( dSegPointDist0) ;
|
|
( DistPointLine( ptCen, trTria.GetP( 1), trTria.GetP( 2))).GetSqDist( dSegPointDist1) ;
|
|
( DistPointLine( ptCen, trTria.GetP( 2), trTria.GetP( 0))).GetSqDist( dSegPointDist2) ;
|
|
if ( dSegPointDist0 > dSqMinDist && dSegPointDist1 > dSqMinDist && dSegPointDist2 > dSqMinDist)
|
|
return false ;
|
|
// interferenza
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Date due rette, descritte da un punto e un vettore direzione,
|
|
// trova i parametri corrispondenti ai punti di minima distanza.
|
|
// Se le rette non sono parallele abbiamo una singola coppia di
|
|
// punti e viene restituito true, altrimenti ne abbiamo infinite
|
|
// e viene restituito false. Si noti che i parametri dU1, dU2
|
|
// non hanno necessariamente il significato di lunghezza;
|
|
// perché non è richiesto che i vettori siano normalizzati.
|
|
bool
|
|
FindLineLineMinDistPar( const Point3d& ptL1, const Vector3d& vtV1,
|
|
const Point3d& ptL2, const Vector3d& vtV2,
|
|
double& dU1, double& dU2)
|
|
{
|
|
// Se le rette sono parallele
|
|
if ( AreSameOrOppositeVectorExact( vtV1, vtV2))
|
|
return false ;
|
|
|
|
// Vettore congiungente i punti iniziali
|
|
Vector3d vtD = ptL2 - ptL1 ;
|
|
// Proiezioni di vtD sui versori direzione
|
|
double dD1 = vtD * vtV1 ;
|
|
double dD2 = vtD * vtV2 ;
|
|
// Prodotto scalare fra i versori direzione
|
|
double dV = vtV1 * vtV2 ;
|
|
|
|
// Se le rette sono perpendicolari
|
|
if ( abs( vtV1 * vtV2) < EPS_ZERO) {
|
|
dU1 = dD1 ;
|
|
dU2 = - dD2 ;
|
|
}
|
|
// Caso generale
|
|
else {
|
|
dU1 = ( dD1 - dD2 * dV) / ( 1 - dV * dV) ;
|
|
dU2 = ( - dD2 + dD1 * dV) / ( 1 - dV * dV) ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Calcolo delle posizioni di tangenza di una sfera che si muove dalla posizione
|
|
// originale lungo una direzione prefissata rispetto ad una retta.
|
|
// Le posizioni sono rappresentate dalla distanza percorsa.
|
|
// Il parametro di ritorno può assumere i seguenti valori :
|
|
// 0 : la sfera non è mai tangente alla retta; dU1 e dU2 non hanno senso
|
|
// 1 : la sfera è tangente alla retta in una posizione; solo dU1 ha senso
|
|
// 2 : la sfera è tangente alla retta in due posizioni; dU1 e dU2 valide
|
|
// 3 : la sfera è sempre tangente
|
|
int
|
|
SphereLineTangentPoints( const Point3d& ptSpheCen, double dSpheRad,
|
|
const Point3d& ptSeg, const Vector3d& vtSegDir, double dSegLen,
|
|
const Vector3d& vtMove, double& dU1, double& dU2)
|
|
{
|
|
// Definisco l'equazione
|
|
Vector3d vtMotOrt = vtMove - vtMove * vtSegDir * vtSegDir ;
|
|
Vector3d vtLineSeg = ptSeg - ptSpheCen ;
|
|
Vector3d vtLineSegOrt = vtLineSeg - vtLineSeg * vtSegDir * vtSegDir ;
|
|
// Setto i coefficienti dell'equazione
|
|
DBLVECTOR vdCoeff( 3) ;
|
|
vdCoeff[0] = vtLineSegOrt.SqLen() - dSpheRad * dSpheRad ;
|
|
vdCoeff[1] = - 2 * vtMotOrt * vtLineSegOrt ;
|
|
vdCoeff[2] = vtMotOrt.SqLen() ;
|
|
if ( vdCoeff[2] < EPS_ZERO * EPS_ZERO) {
|
|
if ( abs( vdCoeff[0]) < EPS_SMALL)
|
|
return 3 ;
|
|
else
|
|
return 0 ;
|
|
}
|
|
// Risolvo l'equazione
|
|
DBLVECTOR vdRoots ;
|
|
int nRoots = PolynomialRoots( 2, vdCoeff, vdRoots) ;
|
|
// Studio le soluzioni
|
|
if ( nRoots == 1)
|
|
dU1 = vdRoots[0] ;
|
|
else if ( nRoots == 2) {
|
|
dU1 = min( vdRoots[0], vdRoots[1]) ;
|
|
dU2 = max( vdRoots[0], vdRoots[1]) ;
|
|
}
|
|
return nRoots ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Valuta l'allontanamento di un disco, lungo il proprio asse di simmetria, da un piano.
|
|
// Il disco è descritto dal centro nella posizione iniziale e dal raggio (la normale è il versore traslazione).
|
|
// Il piano è descritto da un suo punto e dal suo versore normale.
|
|
// Il moto è descritto dal versore di traslazione.
|
|
// Il valore restituito è un intero che descrive la situazione di allontanamento e,
|
|
// quando sensato, per referenza restituisce il punto di ultimo contatto.
|
|
// 0: Nessun contatto
|
|
// 1: Un contatto
|
|
// 2: Disco e piano ortogonali, con disco tg al piano, il contatto è una semiretta
|
|
// 3: Disco e piano ortogonali, con disco secante il piano, il contatto è una striscia
|
|
// 4: Disco e piano paralleli, come ultimo contatto si restituisce il centro.
|
|
// Nel caso 0 il parametro ptTouch non ha senso, così come nel caso 2 e nel caso 3
|
|
int
|
|
DiskPlaneLastContactLongMot( const Point3d& ptDiskCen, double dDiskRad,
|
|
const Point3d& ptPlane, const Vector3d& vtPlane,
|
|
const Vector3d& vtMove, Point3d& ptTouch)
|
|
{
|
|
// Se disco e piano sono paralleli
|
|
if ( AreSameOrOppositeVectorApprox( vtMove, vtPlane)) {
|
|
double dDist = PointPlaneSignedDist( ptPlane, ptDiskCen, vtMove) ;
|
|
if ( dDist > - EPS_SMALL) {
|
|
ptTouch = ptDiskCen + dDist * vtMove ;
|
|
return 4 ;
|
|
}
|
|
return 0 ;
|
|
}
|
|
// Vettore radiale
|
|
Vector3d vtRadLine = vtMove * ( vtPlane * vtMove) - vtPlane ;
|
|
// Disco e piano ortogonali
|
|
if ( vtRadLine.IsNormalized()) {
|
|
double dDist = abs( PointPlaneSignedDist( ptPlane, ptDiskCen, vtMove)) ;
|
|
if ( abs( dDist - dDiskRad) < EPS_SMALL)
|
|
return 2 ;
|
|
else if ( dDist < dDiskRad)
|
|
return 3 ;
|
|
return 0 ;
|
|
}
|
|
// Cerco un punto di contatto nell'interno del triangolo. Se tale punto
|
|
// esiste, la retta intersezione fra il piano del triangolo e quello del
|
|
// disco è tangente alla circonferenza.
|
|
vtRadLine.Normalize() ;
|
|
// Punti delle due rette candidate all'intersezione col triangolo
|
|
Point3d ptStPlus = ptDiskCen + dDiskRad * vtRadLine ;
|
|
Point3d ptStMinus = ptDiskCen - dDiskRad * vtRadLine ;
|
|
// Parametri d'intersezione delle rette col piano
|
|
double dDistPlus = ( ( ptPlane - ptStPlus) * vtPlane) / ( vtMove * vtPlane) ;
|
|
double dDistMinus = ( ( ptPlane - ptStMinus) * vtPlane) / ( vtMove * vtPlane) ;
|
|
double dDist ;
|
|
if ( dDistPlus > dDistMinus) {
|
|
dDist = dDistPlus ;
|
|
ptTouch = ptStPlus + dDist * vtMove ;
|
|
}
|
|
else {
|
|
dDist = dDistMinus ;
|
|
ptTouch = ptStMinus + dDist * vtMove ;
|
|
}
|
|
if ( dDist > - EPS_SMALL)
|
|
return 1 ;
|
|
return 0 ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Si limita il segmento alla parte di spazio con distanza positiva dal piano.
|
|
// Se segmento ancora valido si restituisce true, altrimenti false.
|
|
bool
|
|
ClampSegmentOutPlane( const Plane3d& plPlane, Point3d& ptSegP, const Vector3d& vtSegV, double& dSegLen, double dSignTol)
|
|
{
|
|
// Distanze degli estremi del segmento dal piano
|
|
double dDistStart = DistPointPlane( ptSegP, plPlane) ;
|
|
double dDistEnd = DistPointPlane( ptSegP + dSegLen * vtSegV, plPlane) ;
|
|
// Se il segmento sta nel piano o è tutto esterno, va bene
|
|
if ( dDistStart > dSignTol - EPS_ZERO && dDistEnd > dSignTol - EPS_ZERO)
|
|
return true ;
|
|
// Se il segmento è tutto interno, va eliminato
|
|
if ( dDistStart < dSignTol && dDistEnd < dSignTol) {
|
|
dSegLen = 0 ;
|
|
return false ;
|
|
}
|
|
// Altrimenti il segmento attraversa il piano e va limitato
|
|
double dIntersLen = dSegLen * abs( dDistStart) / ( abs( dDistStart) + abs( dDistEnd)) ;
|
|
if ( dDistStart > 0)
|
|
dSegLen = dIntersLen ;
|
|
else {
|
|
ptSegP += dIntersLen * vtSegV ;
|
|
dSegLen -= dIntersLen ;
|
|
}
|
|
return true ;
|
|
}
|