Riccardo Elitropi
c2a79b2665
- InTestSurfTmSurfTm affinato il controllo collisione quando una delle due superfici è chiusa e contiene l'altra. In CalcPocketing aggiunta piccola miglioria.
231 lines
11 KiB
C++
231 lines
11 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2020-2026
|
|
//----------------------------------------------------------------------------
|
|
// File : CDeClosedSurfTmClosedSurfTm.h Data : 23.01.26 Versione : 3.1a3
|
|
// Contenuto : Implementazione funzione verifica collisione tra
|
|
// SurfTm e SurfTm.
|
|
//
|
|
// Modifiche : 13.11.20 LM Creazione modulo.
|
|
// 24.03.24 DS Aggiunta TestSurfTmSurfTm.
|
|
// 23.01.26 DS In TestSurfTmSurfTm aggiunto flag per collisione quando una delle due è chiusa e contiene l'altra.
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
#include "stdafx.h"
|
|
#include "SurfTriMesh.h"
|
|
#include "CDeTriaTria.h"
|
|
#include "CDeSpheTria.h"
|
|
#include "CDeCylTria.h"
|
|
#include "/EgtDev/Include/EGkBBox3d.h"
|
|
#include "/EgtDev/Include/EGkCDeClosedSurfTmClosedSurfTm.h"
|
|
#include "/EgtDev/Include/EGkDistPointSurfTm.h"
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Le due superfici devono essere espresse nel medesimo sistema di riferimento.
|
|
// La distanza di sicurezza ha effetto solo se maggiore di EPS_SMALL.
|
|
// Se necessario cerco la collisione con un offset della superficie B costituito
|
|
// da sfere centrate nei vertici, cilindri con gli spigoli per asse e il triangolo
|
|
// originale traslato della distanza di sicurezza lungo la sua normale.
|
|
// La funzione restituisce :
|
|
// - true in caso di collisione o inconsistenza dei parametri di input
|
|
// - false in caso di assenza di collisione.
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CDeClosedSurfTmClosedSurfTm( const ISurfTriMesh& SurfA, const ISurfTriMesh& SurfB, double dSafeDist)
|
|
{
|
|
// Recupero le superfici base
|
|
const SurfTriMesh* pSrfA = GetBasicSurfTriMesh( &SurfA) ;
|
|
const SurfTriMesh* pSrfB = GetBasicSurfTriMesh( &SurfB) ;
|
|
// Se le superfici non sono valide o non sono chiuse, non ha senso proseguire
|
|
if ( pSrfA == nullptr || ! pSrfA->IsValid() || ! pSrfA->IsClosed() ||
|
|
pSrfB == nullptr || ! pSrfB->IsValid() || ! pSrfB->IsClosed())
|
|
return true ;
|
|
// Se i box delle superfici non si intersecano, ho finito.
|
|
BBox3d b3BoxA, b3BoxB ;
|
|
pSrfA->GetLocalBBox( b3BoxA) ;
|
|
pSrfB->GetLocalBBox( b3BoxB) ;
|
|
// Se è necessario, espando il box di B di una costante additiva pari alla distanza di sicurezza.
|
|
if ( dSafeDist > EPS_SMALL)
|
|
b3BoxB.Expand( dSafeDist) ;
|
|
// Se i box non si sovrappongono, non c'è collisione. Ho finito.
|
|
if ( ! b3BoxA.Overlaps( b3BoxB))
|
|
return false ;
|
|
// Recupero i triangoli di A che interferiscono col box di B
|
|
INTVECTOR vTriaIndex ;
|
|
pSrfA->GetAllTriaOverlapBox( b3BoxB, vTriaIndex) ;
|
|
// Ciclo sui triangoli della superficie A che interferiscono col box della superficie B.
|
|
for ( int nTA : vTriaIndex) {
|
|
Triangle3d trTriaA ;
|
|
if ( ! ( pSrfA->GetTriangle( nTA, trTriaA) && trTriaA.Validate()))
|
|
continue ;
|
|
BBox3d b3BoxTriaA ;
|
|
trTriaA.GetLocalBBox( b3BoxTriaA) ;
|
|
// Se è necessario, espando il box di una costante additiva pari alla distanza di sicurezza.
|
|
if ( dSafeDist > EPS_SMALL)
|
|
b3BoxTriaA.Expand( dSafeDist) ;
|
|
// Recupero i triangoli di B che interferiscono col box del triangolo di A
|
|
INTVECTOR vNearTria ;
|
|
pSrfB->GetAllTriaOverlapBox( b3BoxTriaA, vNearTria) ;
|
|
// Settare tutti i triangoli come già processati.
|
|
// Al termine della chiamata i TempInt dei triangoli valgono 0.
|
|
pSrfB->ResetTempInts() ;
|
|
// Ciclo sui triangoli della superficie B che cadono nel box del triangolo corrente della Superficie A.
|
|
for ( int nTB : vNearTria) {
|
|
// Recupero il triangolo corrente della superficie B.
|
|
// Se triangolo non valido salto al successivo.
|
|
Triangle3d trTriaB ;
|
|
if ( ! ( pSrfB->GetTriangle( nTB, trTriaB) && trTriaB.Validate()))
|
|
continue ;
|
|
// Se necessario considero l'offset
|
|
if ( dSafeDist > EPS_SMALL) {
|
|
int nAdjTriaId[3] ;
|
|
pSrfB->GetTriangleAdjacencies( nTB, nAdjTriaId) ;
|
|
// Ciclo sui vertici del triangolo.
|
|
for ( int nVB = 0 ; nVB < 3 ; ++ nVB) {
|
|
// Se il triangolo adiacente al triangolo corrente su questo edge
|
|
// non è stato processato, processo il vertice e l'edge.
|
|
int nAdjTriaTempFlag ;
|
|
if ( ! ( pSrfB->GetTempInt( nAdjTriaId[nVB], nAdjTriaTempFlag) || nAdjTriaTempFlag == 0))
|
|
continue ;
|
|
// Processo il vertice: se c'è collisione fra triangolo A e sfera ho finito.
|
|
if ( CDeSimpleSpheTria( trTriaB.GetP( nVB), dSafeDist, trTriaA))
|
|
return true ;
|
|
// Processo l'edge: se c'è collisione fra triangolo A e cilindro ho finito.
|
|
Vector3d vtEdgeV = trTriaB.GetP( ( nVB + 1) % 3) - trTriaB.GetP( nVB) ;
|
|
double dEdgeLen = vtEdgeV.Len() ;
|
|
vtEdgeV /= dEdgeLen ;
|
|
Frame3d frCyl ;
|
|
frCyl.Set( trTriaB.GetP( nVB), vtEdgeV) ;
|
|
if ( CDeSimpleCylTria( frCyl, dSafeDist, dEdgeLen, trTriaA))
|
|
return true ;
|
|
}
|
|
// Traslo il triangolo
|
|
trTriaB.Translate( dSafeDist * trTriaB.GetN()) ;
|
|
}
|
|
// Processo il triangolo: se i due triangoli collidono ho finito.
|
|
if ( CDeTriaTria( trTriaA, trTriaB))
|
|
return true ;
|
|
// Segno il triangolo come processato: nTemp = 1
|
|
pSrfB->SetTempInt( nTB, 1) ;
|
|
}
|
|
}
|
|
// Non ho trovato collisioni fra triangoli delle superfici.
|
|
// Se il BBox della prima superficie non è interno a quello della seconda e viceversa, non c'è collisione
|
|
if ( ! b3BoxA.Encloses( b3BoxB) && ! b3BoxB.Encloses( b3BoxA))
|
|
return false ;
|
|
// La collisione c'è se una superficie è dentro l'altra.
|
|
Point3d ptPointA, ptPointB ;
|
|
pSrfA->GetFirstVertex( ptPointA) ;
|
|
pSrfB->GetFirstVertex( ptPointB) ;
|
|
DistPointSurfTm DistPoinASrfB( ptPointA, *pSrfB) ;
|
|
DistPointSurfTm DistPoinBSrfA( ptPointB, *pSrfA) ;
|
|
return ( DistPoinASrfB.IsPointInside() || DistPoinBSrfA.IsPointInside()) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Verifica l'interferenza tra le due superfici : restituisce true in caso di interferenza.
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
TestSurfTmSurfTm( const ISurfTriMesh& SurfA, const ISurfTriMesh& SurfB, double dSafeDist, bool bTestEnclosion)
|
|
{
|
|
// Recupero le superfici base
|
|
const SurfTriMesh* pSrfA = GetBasicSurfTriMesh( &SurfA) ;
|
|
const SurfTriMesh* pSrfB = GetBasicSurfTriMesh( &SurfB) ;
|
|
// Se le superfici non sono valide, non ha senso proseguire
|
|
if ( pSrfA == nullptr || ! pSrfA->IsValid() ||
|
|
pSrfB == nullptr || ! pSrfB->IsValid())
|
|
return true ;
|
|
// Se i box delle superfici non si intersecano, ho finito.
|
|
BBox3d b3BoxA, b3BoxB ;
|
|
pSrfA->GetLocalBBox( b3BoxA) ;
|
|
pSrfB->GetLocalBBox( b3BoxB) ;
|
|
// Se è necessario, espando il box di B di una costante additiva pari alla distanza di sicurezza.
|
|
if ( dSafeDist > EPS_SMALL)
|
|
b3BoxB.Expand( dSafeDist) ;
|
|
// Se i box non si sovrappongono, non c'è collisione. Ho finito.
|
|
if ( ! b3BoxA.Overlaps( b3BoxB))
|
|
return false ;
|
|
// Recupero i triangoli di A che interferiscono col box di B
|
|
INTVECTOR vTriaIndex ;
|
|
pSrfA->GetAllTriaOverlapBox( b3BoxB, vTriaIndex) ;
|
|
// Ciclo sui triangoli della superficie A che interferiscono col box della superficie B.
|
|
for ( int nTA : vTriaIndex) {
|
|
Triangle3d trTriaA ;
|
|
if ( ! ( pSrfA->GetTriangle( nTA, trTriaA) && trTriaA.Validate()))
|
|
continue ;
|
|
BBox3d b3BoxTriaA ;
|
|
trTriaA.GetLocalBBox( b3BoxTriaA) ;
|
|
// Se è necessario, espando il box di una costante additiva pari alla distanza di sicurezza.
|
|
if ( dSafeDist > EPS_SMALL)
|
|
b3BoxTriaA.Expand( dSafeDist) ;
|
|
// Recupero i triangoli di B che interferiscono col box del triangolo di A
|
|
INTVECTOR vNearTria ;
|
|
pSrfB->GetAllTriaOverlapBox( b3BoxTriaA, vNearTria) ;
|
|
// Settare tutti i triangoli come già processati.
|
|
// Al termine della chiamata i TempInt dei triangoli valgono 0.
|
|
pSrfB->ResetTempInts() ;
|
|
// Ciclo sui triangoli della superficie B che cadono nel box del triangolo corrente della Superficie A.
|
|
for ( int nTB : vNearTria) {
|
|
// Recupero il triangolo corrente della superficie B.
|
|
// Se triangolo non valido salto al successivo.
|
|
Triangle3d trTriaB ;
|
|
if ( ! ( pSrfB->GetTriangle( nTB, trTriaB) && trTriaB.Validate()))
|
|
continue ;
|
|
// Se necessario considero l'offset
|
|
if ( dSafeDist > EPS_SMALL) {
|
|
int nAdjTriaId[3] ;
|
|
pSrfB->GetTriangleAdjacencies( nTB, nAdjTriaId) ;
|
|
// Ciclo sui vertici del triangolo.
|
|
for ( int nVB = 0 ; nVB < 3 ; ++ nVB) {
|
|
// Se il triangolo adiacente al triangolo corrente su questo edge
|
|
// non è stato processato, processo il vertice e l'edge.
|
|
int nAdjTriaTempFlag ;
|
|
if ( ! ( pSrfB->GetTempInt( nAdjTriaId[nVB], nAdjTriaTempFlag) || nAdjTriaTempFlag == 0))
|
|
continue ;
|
|
// Processo il vertice: se c'è collisione fra triangolo A e sfera ho finito.
|
|
if ( CDeSimpleSpheTria( trTriaB.GetP( nVB), dSafeDist, trTriaA))
|
|
return true ;
|
|
// Processo l'edge: se c'è collisione fra triangolo A e cilindro ho finito.
|
|
Vector3d vtEdgeV = trTriaB.GetP( ( nVB + 1) % 3) - trTriaB.GetP( nVB) ;
|
|
double dEdgeLen = vtEdgeV.Len() ;
|
|
vtEdgeV /= dEdgeLen ;
|
|
Frame3d frCyl ;
|
|
frCyl.Set( trTriaB.GetP( nVB), vtEdgeV) ;
|
|
if ( CDeSimpleCylTria( frCyl, dSafeDist, dEdgeLen, trTriaA))
|
|
return true ;
|
|
}
|
|
// Traslo il triangolo
|
|
trTriaB.Translate( dSafeDist * trTriaB.GetN()) ;
|
|
}
|
|
// Processo il triangolo: se i due triangoli collidono ho finito.
|
|
if ( CDeTriaTria( trTriaA, trTriaB))
|
|
return true ;
|
|
// Segno il triangolo come processato: nTemp = 1
|
|
pSrfB->SetTempInt( nTB, 1) ;
|
|
}
|
|
}
|
|
// Se non richiesto test di inclusione, non c'è interferenza
|
|
if ( ! bTestEnclosion)
|
|
return false ;
|
|
// Se la prima superficie è chiusa, verifico se include totalmente la seconda
|
|
if ( pSrfA->IsClosed() && b3BoxA.Encloses( b3BoxB)) {
|
|
Point3d ptPointB ;
|
|
pSrfB->GetFirstVertex( ptPointB) ;
|
|
DistPointSurfTm DistPoinBSrfA( ptPointB, *pSrfA) ;
|
|
if ( DistPoinBSrfA.IsPointInside() || DistPoinBSrfA.IsEpsilon( dSafeDist))
|
|
return true ;
|
|
}
|
|
// Se la seconda superficie è chiusa, verifico se include totalmente la prima
|
|
if ( pSrfB->IsClosed() && b3BoxB.Encloses( b3BoxA)) {
|
|
Point3d ptPointA ;
|
|
pSrfA->GetFirstVertex( ptPointA) ;
|
|
DistPointSurfTm DistPoinASrfB( ptPointA, *pSrfB) ;
|
|
if ( DistPoinASrfB.IsPointInside() || DistPoinASrfB.IsEpsilon( dSafeDist))
|
|
return true ;
|
|
}
|
|
// Non c'è interferenza
|
|
return false ;
|
|
}
|