Dario Sassi
17759877d4
- interfaccia DistLineLine ora esportata - in lettura Curve Composite allargata tolleranza giunzione tra componenti.
185 lines
8.0 KiB
C++
185 lines
8.0 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2020-2020
|
|
//----------------------------------------------------------------------------
|
|
// File : CDeTriaTria.cpp Data : 05.11.20 Versione : 2.2k1
|
|
// Contenuto : Implementazione della verifica di collisione tra due Triangle3d.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 05.11.20 LM Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
#include "stdafx.h"
|
|
#include "CDeTriaTria.h"
|
|
#include "/EgtDev/Include/EGkDistLineLine.h"
|
|
#include "/EgtDev/Include/EGkIntersPlanePlane.h"
|
|
#include <array>
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
void
|
|
GetTwoGreater( int nVal0, int nVal1, int nVal2, int& nFirstMaxPos, int& nSecondMaxPos)
|
|
{
|
|
if ( nVal0 > nVal1) {
|
|
if ( nVal0 > nVal2) {
|
|
nFirstMaxPos = 0 ;
|
|
if ( nVal1 > nVal2)
|
|
nSecondMaxPos = 1 ;
|
|
else
|
|
nSecondMaxPos = 2 ;
|
|
}
|
|
else {
|
|
nFirstMaxPos = 2 ;
|
|
nSecondMaxPos = 0 ;
|
|
}
|
|
}
|
|
else if ( nVal0 > nVal2) {
|
|
nFirstMaxPos = 1 ;
|
|
nSecondMaxPos = 0 ;
|
|
}
|
|
else if ( nVal1 > nVal2) {
|
|
nFirstMaxPos = 1 ;
|
|
nSecondMaxPos = 2 ;
|
|
}
|
|
else {
|
|
nFirstMaxPos = 2 ;
|
|
nSecondMaxPos = 1 ;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CDeTriaTria( const Triangle3d& trTriaA, const Triangle3d& trTriaB)
|
|
{
|
|
// Piano del triangolo A
|
|
Plane3d plPlaneTriaA ;
|
|
plPlaneTriaA.Set( trTriaA.GetCentroid(), trTriaA.GetN()) ;
|
|
// Calcolo le distanze dei vertici del triangolo B dal piano del triangolo A
|
|
array< double, 3> vDistBA ;
|
|
for ( int i = 0 ; i < 3 ; ++ i)
|
|
vDistBA[i] = DistPointPlane( trTriaB.GetP( i), plPlaneTriaA) ;
|
|
// Verifico posizione del triangolo B rispetto al piano del triangolo A
|
|
int nVertPosB = 0 ;
|
|
int nVertNegB = 0 ;
|
|
for ( const auto& dDist : vDistBA) {
|
|
if ( dDist > EPS_SMALL)
|
|
++ nVertPosB ;
|
|
else if ( dDist < - EPS_SMALL)
|
|
++ nVertNegB ;
|
|
}
|
|
// Se il triangolo giace tutto da una parte del piano, nessuna intersezione
|
|
if ( nVertPosB == 3 || nVertNegB == 3)
|
|
return false ;
|
|
// Piano del triangolo B
|
|
Plane3d plPlaneTriB ;
|
|
plPlaneTriB.Set( trTriaB.GetCentroid(), trTriaB.GetN()) ;
|
|
// Calcolo le distanze dei vertici del triangolo A dal piano del triangolo B
|
|
array< double, 3> vDistAB ;
|
|
for ( int i = 0 ; i < 3 ; ++ i)
|
|
vDistAB[i] = DistPointPlane( trTriaA.GetP(i) , plPlaneTriB) ;
|
|
// Verifico posizione del triangolo A rispetto al piano del triangolo B
|
|
int nVertPosA = 0 ;
|
|
int nVertNegA = 0 ;
|
|
for ( const auto& dDist : vDistAB) {
|
|
if ( dDist > EPS_SMALL)
|
|
++ nVertPosA ;
|
|
else if ( dDist < - EPS_SMALL)
|
|
++ nVertNegA ;
|
|
}
|
|
// Se il triangolo giace tutto da una parte del piano, nessuna intersezione
|
|
if ( nVertPosA == 3 || nVertNegA == 3)
|
|
return false ;
|
|
// Interseco i piani
|
|
Point3d ptLineP ;
|
|
Vector3d vtLineV ;
|
|
int nResPP = IntersPlanePlane( plPlaneTriaA, plPlaneTriB, ptLineP, vtLineV) ;
|
|
if ( nResPP == IPPT_NO)
|
|
return false ;
|
|
else if ( nResPP == IPPT_OVERLAPS) {
|
|
for ( int nA = 0 ; nA < 3 ; ++ nA) {
|
|
Point3d ptStA = trTriaA.GetP( nA) ;
|
|
Point3d PtEnA = trTriaA.GetP( ( nA + 1) % 3);
|
|
for ( int nB = 0 ; nB < 3 ; ++ nB) {
|
|
Point3d ptStB = trTriaB.GetP( nB) ;
|
|
Point3d PtEnB = trTriaB.GetP( ( nB + 1) % 3) ;
|
|
DistLineLine LineLineDistCalc( ptStA, PtEnA, ptStB, PtEnB) ;
|
|
double dSqSegSegDist ;
|
|
LineLineDistCalc.GetSqDist( dSqSegSegDist) ;
|
|
if ( dSqSegSegDist < SQ_EPS_SMALL)
|
|
return true ;
|
|
}
|
|
}
|
|
return ( IsPointInsideTriangle( trTriaA.GetP( 0), trTriaB, TriangleType::CLOSED) ||
|
|
IsPointInsideTriangle( trTriaA.GetP( 1), trTriaB, TriangleType::CLOSED) ||
|
|
IsPointInsideTriangle( trTriaA.GetP( 2), trTriaB, TriangleType::CLOSED) ||
|
|
IsPointInsideTriangle( trTriaB.GetP( 0), trTriaA, TriangleType::CLOSED) ||
|
|
IsPointInsideTriangle( trTriaB.GetP( 1), trTriaA, TriangleType::CLOSED) ||
|
|
IsPointInsideTriangle( trTriaB.GetP( 2), trTriaA, TriangleType::CLOSED)) ;
|
|
}
|
|
// Interseco la retta col triangolo A
|
|
// Cerco i due segmenti del triangolo A che limitano la retta di intersezione fra i piani
|
|
int nSegExtrDistA[3] = { -1, -1, -1 } ;
|
|
for ( int nV = 0 ; nV < 3 ; ++ nV) {
|
|
int nStateSt = ( vDistAB[nV] > EPS_SMALL ? 1 : ( vDistAB[nV] > - EPS_SMALL ? 0 : - 1)) ;
|
|
int nStateEn = ( vDistAB[( nV + 1) % 3] > EPS_SMALL ? 1 : ( vDistAB[( nV + 1) % 3] > - EPS_SMALL ? 0 : - 1)) ;
|
|
nSegExtrDistA[nV] = abs( nStateSt - nStateEn) ;
|
|
}
|
|
int nFirstMaxPosA, nSecondMaxPosA ;
|
|
GetTwoGreater( nSegExtrDistA[0], nSegExtrDistA[1], nSegExtrDistA[2], nFirstMaxPosA, nSecondMaxPosA) ;
|
|
// Limito la retta col primo segmento trovato di A
|
|
Vector3d vtSegFirstA = trTriaA.GetP( ( nFirstMaxPosA + 1) % 3) - trTriaA.GetP( nFirstMaxPosA) ;
|
|
double dSegLenFirstA = vtSegFirstA.Len() ;
|
|
vtSegFirstA /= dSegLenFirstA ;
|
|
DistLineLine LineLineDistCalcFirstA( ptLineP, vtLineV, 100., trTriaA.GetP( nFirstMaxPosA), vtSegFirstA, dSegLenFirstA, false) ;
|
|
double dIntParStA, dOtherParFirstA ;
|
|
LineLineDistCalcFirstA.GetPositionsAtMinDistPoints( dIntParStA, dOtherParFirstA) ;
|
|
// Limito la retta col secondo segmento trovato di A
|
|
Vector3d vtSegSecondA = trTriaA.GetP( ( nSecondMaxPosA + 1) % 3) - trTriaA.GetP( nSecondMaxPosA) ;
|
|
double dSegLenSecondA = vtSegSecondA.Len() ;
|
|
vtSegSecondA /= dSegLenSecondA ;
|
|
DistLineLine LineLineDistCalcSecondA( ptLineP, vtLineV, 100., trTriaA.GetP( nSecondMaxPosA), vtSegSecondA, dSegLenSecondA, false) ;
|
|
double dIntParEnA, dOtherParSecondA ;
|
|
LineLineDistCalcSecondA.GetPositionsAtMinDistPoints( dIntParEnA, dOtherParSecondA) ;
|
|
// Ordino i parametri lungo la retta di intersezione fra i piani
|
|
if ( dIntParStA > dIntParEnA) {
|
|
swap( dIntParStA, dIntParEnA) ;
|
|
}
|
|
// Interseco la retta col triangolo B
|
|
int nSegExtrDistB[3] = { -1, -1, -1 } ;
|
|
for (int nV = 0; nV < 3; ++nV) {
|
|
int nStateSt = ( vDistBA[nV] > EPS_SMALL ? 1 : ( vDistBA[nV] > - EPS_SMALL ? 0 : - 1)) ;
|
|
int nStateEn = ( vDistBA[( nV + 1) % 3] > EPS_SMALL ? 1 : ( vDistBA[( nV + 1) % 3] > - EPS_SMALL ? 0 : - 1)) ;
|
|
nSegExtrDistB[nV] = abs( nStateSt - nStateEn) ;
|
|
}
|
|
int nFirstMaxPosB, nSecondMaxPosB ;
|
|
GetTwoGreater( nSegExtrDistB[0], nSegExtrDistB[1], nSegExtrDistB[2], nFirstMaxPosB, nSecondMaxPosB) ;
|
|
// Limito la retta col primo segmento trovato di B
|
|
Vector3d vtSegFirstB = trTriaB.GetP( ( nFirstMaxPosB + 1) % 3) - trTriaB.GetP( nFirstMaxPosB) ;
|
|
double dSegLenFirstB = vtSegFirstB.Len() ;
|
|
vtSegFirstB /= dSegLenFirstB ;
|
|
DistLineLine LineLineDistCalcFirstB( ptLineP, vtLineV, 100., trTriaB.GetP( nFirstMaxPosB), vtSegFirstB, dSegLenFirstB, false) ;
|
|
double dIntParStB, dOtherParFirstB ;
|
|
LineLineDistCalcFirstB.GetPositionsAtMinDistPoints( dIntParStB, dOtherParFirstB) ;
|
|
// Limito la retta col secondo segmento trovato di B
|
|
Vector3d vtSegSecondB = trTriaB.GetP( ( nSecondMaxPosB + 1) % 3) - trTriaB.GetP( nSecondMaxPosB) ;
|
|
double dSegLenSecondB = vtSegSecondB.Len() ;
|
|
vtSegSecondB /= dSegLenSecondB ;
|
|
DistLineLine LineLineDistCalcSecondB( ptLineP, vtLineV, 100., trTriaB.GetP( nSecondMaxPosB), vtSegSecondB, dSegLenSecondB, false) ;
|
|
double dIntParEnB, dOtherParSecondB ;
|
|
LineLineDistCalcSecondB.GetPositionsAtMinDistPoints( dIntParEnB, dOtherParSecondB) ;
|
|
// Ordino i parametri lungo la retta di intersezione fra i piani
|
|
if ( dIntParStB > dIntParEnB) {
|
|
swap( dIntParStB, dIntParEnB) ;
|
|
}
|
|
// Se gli intervalli si sovrappongono c'è collisione, altrimenti non c'è
|
|
if ( dIntParEnB < dIntParStA - EPS_SMALL)
|
|
return false ;
|
|
else if ( dIntParStB < dIntParEnA + EPS_SMALL)
|
|
return true ;
|
|
else
|
|
return false ;
|
|
}
|