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).
104 lines
3.9 KiB
C++
104 lines
3.9 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2022-2022
|
|
//----------------------------------------------------------------------------
|
|
// File : CDeCapsTria.cpp Data : 14.05.22 Versione : 2.4e2
|
|
// Contenuto : Implementazione della verifica di collisione tra
|
|
// Capsule (cilindro con estremità semisferiche) e Triangle3d.
|
|
//
|
|
//
|
|
// Modifiche :14.05.22 DS Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
//--------------------------- Include ----------------------------------------
|
|
#include "stdafx.h"
|
|
#include "CDeCapsTria.h"
|
|
#include "CDeSpheTria.h"
|
|
#include "ProjPlane.h"
|
|
#include "IntersLineCaps.h"
|
|
#include "/EgtDev/Include/EGkPolygon3d.h"
|
|
#include "/EgtDev/Include/EGkIntersLinePlane.h"
|
|
#include "/EgtDev/Include/EGkDistPointTria.h"
|
|
#include "/EgtDev/Include/EGkIntersLineSphere.h"
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CDeSimpleCapsTria( const Point3d& ptP1, const Point3d& ptP2, double dR, const Triangle3d& trTria)
|
|
{
|
|
// vedi Ericson, Real-Time Collision Detection, pag. 226 (Nettle method)
|
|
|
|
// Dati della capsule come sfera che si muove
|
|
Point3d ptC = ptP1 ;
|
|
Point3d ptE = ptP2 ;
|
|
Vector3d vtDir = ptP2 - ptP1 ;
|
|
double dLen = vtDir.Len() ;
|
|
if ( dLen < EPS_SMALL)
|
|
return CDeSimpleSpheTria( Media( ptP1, ptP2), dR, trTria) ;
|
|
vtDir /= dLen ;
|
|
if ( vtDir * trTria.GetN() > 0) {
|
|
vtDir.Invert() ;
|
|
ptC = ptP2 ;
|
|
ptE = ptP1 ;
|
|
}
|
|
// Se sfera finale dista dal piano come o meno del raggio, devo verificarla direttamente (il retro è escluso dal calcolo standard)
|
|
double dDistE = DistPointPlane( ptE, trTria.GetPlane()) ;
|
|
if ( abs( dDistE) <= dR) {
|
|
if ( CDeSimpleSpheTria( ptE, dR, trTria))
|
|
return true ;
|
|
}
|
|
// Determinazione primo possibile punto di contatto della sfera con il piano
|
|
Point3d ptD = ptC - trTria.GetN() * dR ;
|
|
// Intersezione della linea di movimento di questo punto con il piano del triangolo
|
|
Point3d ptP ;
|
|
int nLpRes = IntersLinePlane( ptD, vtDir, 1, trTria.GetPlane(), ptP, false) ;
|
|
// Se non c'è intersezione passante
|
|
if ( nLpRes != ILPT_YES) {
|
|
// se il centro dista dal piano non meno del raggio, allora non c'è sicuramente collisione
|
|
double dDistM = DistPointPlane( Media( ptP1, ptP2), trTria.GetPlane()) ;
|
|
if ( abs( dDistM) >= dR)
|
|
return false ;
|
|
// interseco l'asse del capsule con capsule di pari raggio con asse i lati del triangolo
|
|
double dU1 = INFINITO, dU2 = -INFINITO ;
|
|
for ( int i = 0 ; i < 3 ; ++ i) {
|
|
double dInt1, dInt2 ;
|
|
if ( IntersLineCaps( ptC, vtDir, trTria.GetP( i), trTria.GetP( ( i + 1) % 3), dR, dInt1, dInt2)) {
|
|
dU1 = min( dU1, dInt1) ;
|
|
dU2 = max( dU2, dInt2) ;
|
|
if ( ! ( dU1 >= dLen || dU2 <= 0))
|
|
return true ;
|
|
}
|
|
}
|
|
return false ;
|
|
}
|
|
// Determino la posizione dell'intersezione rispetto al triangolo
|
|
DistPointTriangle dptDist( ptP, trTria) ;
|
|
// Se l'intersezione sta nel triangolo
|
|
double dSqDist ;
|
|
if ( dptDist.GetSqDist( dSqDist) && dSqDist < 4 * SQ_EPS_SMALL) {
|
|
double dPos = ( ptP - ptD) * vtDir ;
|
|
return ( dPos > 0 && dPos < dLen) ;
|
|
}
|
|
// Altrimenti, recupero il punto del triangolo più vicino all'intersezione
|
|
Point3d ptQ ;
|
|
if ( dptDist.GetMinDistPoint( ptQ)) {
|
|
Point3d ptI1, ptI2 ;
|
|
int nLsRes = IntersLineSphere( ptQ, -vtDir, ptC, dR, ptI1, ptI2) ;
|
|
if ( nLsRes != ILST_SEC)
|
|
return false ;
|
|
double dPos = ( ptQ - ptI1) * vtDir ;
|
|
return ( dPos > 0 && dPos < dLen) ;
|
|
}
|
|
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
CDeCapsTria( const Point3d& ptP1, const Point3d& ptP2, double dR, const Triangle3d& trTria, double dSafeDist)
|
|
{
|
|
return CDeSimpleCapsTria( ptP1, ptP2, dR + max( 0., dSafeDist), trTria) ;
|
|
}
|
|
|