Dario Sassi
ddade325c4
- standardizzate le funzione Collision Detection sia per trimesh sia per Zmap (ex Avoid...) - nelle funzioni Cde ora se arrivano geometrie errate si ritorna collisione (maggior sicurezza).
115 lines
4.2 KiB
C++
115 lines
4.2 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2020-2020
|
|
//----------------------------------------------------------------------------
|
|
// File : CDeCylTria.cpp Data : 08.01.20 Versione : 2.2a2
|
|
// Contenuto : Implementazione della verifica di collisione tra
|
|
// Cylinder e Triangle3d.
|
|
//
|
|
//
|
|
// Modifiche :08.01.20 DS Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
//--------------------------- Include ----------------------------------------
|
|
#include "stdafx.h"
|
|
#include "DistPointLine.h"
|
|
#include "CDeCylTria.h"
|
|
#include "CDeConvexTorusTria.h"
|
|
#include "/EgtDev/Include/EGkPolygon3d.h"
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CDeSimpleCylTria( const Frame3d& frCyl, double dR, double dH, const Triangle3d& trTria)
|
|
{
|
|
// Costruisco il poligono equivalente
|
|
Polygon3d plyTria ;
|
|
if ( ! plyTria.FromTriangle( trTria))
|
|
return false ;
|
|
// Lo porto nel riferimento del cilindro
|
|
plyTria.ToLoc( frCyl) ;
|
|
// Lo taglio come il cilindro
|
|
Plane3d plTrimB ;
|
|
plTrimB.Set( 0., -Z_AX) ;
|
|
plyTria.Trim( plTrimB, true, false, false) ;
|
|
Plane3d plTrimT ;
|
|
plTrimT.Set( dH, Z_AX) ;
|
|
plyTria.Trim( plTrimT, true, false, false) ;
|
|
// Se non è rimasto alcunchè, allora non c'è collisione
|
|
if ( plyTria.GetSideCount() == 0)
|
|
return false ;
|
|
// Proietto il poligono sul piano XY
|
|
plyTria.Scale( GLOB_FRM, 1, 1, 0) ;
|
|
// Se la proiezione ha area nulla
|
|
if ( abs( plyTria.GetArea()) < SQ_EPS_SMALL) {
|
|
const PNTVECTOR& vVert = plyTria.GetVertices() ;
|
|
for ( int i = 0 ; i < int( vVert.size()) ; ++ i) {
|
|
int j = ( i + 1) % vVert.size() ;
|
|
DistPointLine dstPL( ORIG, vVert[i], vVert[j]) ;
|
|
double dSqDist ;
|
|
if ( dstPL.GetSqDist( dSqDist)) {
|
|
// se distanza minore del raggio, allora collide
|
|
if ( dSqDist <= dR * dR)
|
|
return true ;
|
|
}
|
|
}
|
|
return false ;
|
|
}
|
|
// Verifico che sia orientato verso l'alto
|
|
if ( plyTria.GetVersN().z < -EPS_SMALL)
|
|
plyTria.Invert() ;
|
|
// Determino la distanza dell'origine del piano dai lati del poligono convesso
|
|
const PNTVECTOR& vVert = plyTria.GetVertices() ;
|
|
int nAtLeft = 0 ;
|
|
for ( int i = 0 ; i < int( vVert.size()) ; ++ i) {
|
|
int j = ( i + 1) % vVert.size() ;
|
|
DistPointLine dstPL( ORIG, vVert[i], vVert[j]) ;
|
|
double dSqDist ;
|
|
if ( dstPL.GetSqDist( dSqDist)) {
|
|
// se distanza minore del raggio, allora collide
|
|
if ( dSqDist <= dR * dR)
|
|
return true ;
|
|
Point3d ptMinDist ;
|
|
dstPL.GetMinDistPoint( ptMinDist) ;
|
|
// verifico se sta a sinistra
|
|
if ( CrossXY( ( ORIG - ptMinDist), ( vVert[j] - vVert[i])) < 0)
|
|
++ nAtLeft ;
|
|
}
|
|
}
|
|
// Se l'origine è a sinistra di tutti i lati, allora è interna e quindi c'è collisione
|
|
return ( nAtLeft == vVert.size()) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CDeCylTria( const Frame3d& frCyl, double dR, double dH, const Triangle3d& trTria, double dSafeDist)
|
|
{
|
|
// Se distanza di sicurezza nulla
|
|
if ( dSafeDist < EPS_SMALL)
|
|
return CDeSimpleCylTria( frCyl, dR, dH, trTria) ;
|
|
|
|
// Verifica preliminare con cilindro esteso
|
|
Frame3d frEst = frCyl ; frEst.Translate( -dSafeDist * frCyl.VersZ()) ;
|
|
if ( ! CDeSimpleCylTria( frEst, dR + dSafeDist, dH + 2 * dSafeDist, trTria))
|
|
return false ;
|
|
|
|
// Cilindro allargato
|
|
if ( CDeSimpleCylTria( frCyl, dR + dSafeDist, dH, trTria))
|
|
return true ;
|
|
// Cilindro allungato
|
|
Frame3d frTmp = frCyl ; frTmp.Translate( -dSafeDist * frCyl.VersZ()) ;
|
|
if ( CDeSimpleCylTria( frTmp, dR, dH + 2 * dSafeDist, trTria))
|
|
return true ;
|
|
// Toro inferiore
|
|
if ( CDeSimpleConvexTorusTria( frCyl, dR, dSafeDist, CT_INF, trTria))
|
|
return true ;
|
|
// Toro superiore
|
|
frTmp = frCyl ; frTmp.Translate( dH * frCyl.VersZ()) ;
|
|
if ( CDeSimpleConvexTorusTria( frTmp, dR, dSafeDist, CT_SUP, trTria))
|
|
return true ;
|
|
// Non c'è collisione
|
|
return false ;
|
|
}
|