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).
193 lines
8.4 KiB
C++
193 lines
8.4 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2020-2020
|
|
//----------------------------------------------------------------------------
|
|
// File : CDeConTria.cpp Data : 27.10.20 Versione : 2.2k1
|
|
// Contenuto : Implementazione della verifica di collisione tra
|
|
// Cono e Triangle3d.
|
|
//
|
|
//
|
|
// Modifiche : 27.10.20 LM Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
//--------------------------- Include ----------------------------------------
|
|
#include "stdafx.h"
|
|
#include "CDeUtility.h"
|
|
#include "CDeConeFrustumTria.h"
|
|
#include "CDeConeTria.h"
|
|
#include "CDeCylTria.h"
|
|
#include "CDeConvexTorusTria.h"
|
|
#include "IntersLineSurfStd.h"
|
|
#include "/EgtDev/Include/EGkPlane3d.h"
|
|
#include "/EgtDev/Include/EGkIntersLineTria.h"
|
|
#include "/EgtDev/Include/EGkIntersPlanePlane.h"
|
|
#include "/EgtDev/Include/EgtNumUtils.h"
|
|
|
|
using namespace std ;
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Il sistema di riferimento deve avere l'origine nel centro della base minore e l'asse
|
|
// di simmetria del cono, rivolto verso la direzione di apertura, come asse Z.
|
|
bool
|
|
CDeSimpleConeFrustumTria( const Frame3d& frCone, double dMinRad, double dMaxRad, double dHeight, const Triangle3d& trTria)
|
|
{
|
|
// Porto il triangolo nel sistema di riferimento del tronco di cono
|
|
Triangle3d trMyTria = trTria ;
|
|
trMyTria.ToLoc( frCone) ;
|
|
|
|
// Se almeno un vertice collide ho finito
|
|
for ( int nV = 0 ; nV < 3 ; ++ nV) {
|
|
Point3d ptVert = trMyTria.GetP( nV) ;
|
|
double dLenZ = ptVert.z ;
|
|
double dLenXY = sqrt( ptVert.x * ptVert.x + ptVert.y * ptVert.y) ;
|
|
double dRadAtHeight = Clamp( dMinRad + ( dMaxRad - dMinRad) * dLenZ / dHeight, dMinRad, dMaxRad) ;
|
|
if ( dLenZ > - EPS_SMALL && dLenZ < dHeight + EPS_SMALL && dLenXY < dRadAtHeight)
|
|
return true ;
|
|
}
|
|
|
|
// Se almeno un segmento collide ho finito
|
|
for ( int nS = 0 ; nS < 3 ; ++ nS) {
|
|
Point3d ptSegSt = trMyTria.GetP( nS) ;
|
|
Point3d ptSegEn = trMyTria.GetP( ( nS + 1) % 3) ;
|
|
Vector3d vtDir = ptSegEn - ptSegSt ;
|
|
double dSegLen = vtDir.Len() ;
|
|
vtDir /= dSegLen ;
|
|
double dU1, dU2 ;
|
|
// Collisione con la superficie laterale
|
|
int nIndex = SegmentConeFrustum( ptSegSt, vtDir, dSegLen, ORIG, Z_AX, dMinRad, dMaxRad, dHeight, dU1, dU2) ;
|
|
if ( nIndex != CC_ERROR_INT && nIndex != CC_NO_INTERS)
|
|
return true ;
|
|
// Collisione con le basi
|
|
nIndex = SegmentDisc( ptSegSt, vtDir, dSegLen, ORIG, -Z_AX, dMinRad, dU1, dU2) ;
|
|
if ( nIndex != D_ERROR_INT && nIndex != D_NO_INTERS)
|
|
return true ;
|
|
nIndex = SegmentDisc( ptSegSt, vtDir, dSegLen, Point3d( 0., 0., dHeight), Z_AX, dMaxRad, dU1, dU2) ;
|
|
if ( nIndex != D_ERROR_INT && nIndex != D_NO_INTERS)
|
|
return true ;
|
|
}
|
|
|
|
// Contatti con l'interno del triangolo
|
|
Point3d ptP = trMyTria.GetP( 0) ;
|
|
Vector3d vtN = trMyTria.GetN() ;
|
|
// Se il segmento congiungente i centri delle basi interseca il triangolo ho finito
|
|
if ( abs( vtN.z) > EPS_ZERO) {
|
|
Point3d ptInt( 0., 0., ( ( ptP - ORIG) * vtN) / vtN.z) ;
|
|
if ( ptInt.z > - EPS_SMALL && ptInt.z < dHeight + EPS_SMALL &&
|
|
IsPointInsideTriangle( ptInt, trMyTria, TriangleType::CLOSED))
|
|
return true ;
|
|
}
|
|
// Intersezione basi / interno triangolo
|
|
Point3d ptLine ;
|
|
Vector3d vtLine ;
|
|
Plane3d plPlaneTria ;
|
|
plPlaneTria.Set( ptP, vtN) ;
|
|
// Base minore
|
|
Plane3d plMinBase ;
|
|
plMinBase.Set( ORIG, - Z_AX) ;
|
|
int nMinBaseTriaIndex = IntersPlanePlane( plPlaneTria, plMinBase, ptLine, vtLine) ;
|
|
if ( nMinBaseTriaIndex == IPPT_OVERLAPS) {
|
|
if ( CoplanarDiscTriangleInterference( plMinBase.GetPoint(), dMinRad, trMyTria, TriangleType::CLOSED))
|
|
return true ;
|
|
return false ;
|
|
}
|
|
else if ( nMinBaseTriaIndex == IPPT_YES) {
|
|
double dU1, dU2 ;
|
|
if ( LineDisc( ptLine, vtLine, ORIG, -Z_AX, dMinRad, dU1, dU2) == D_INFINITE_INT_LINE_ON_PLANE) {
|
|
Point3d ptInt, ptInt2 ;
|
|
int nLineTriaIndex = IntersLineTria( ptLine + dU1 * vtLine, ptLine + dU2 * vtLine, trMyTria, ptInt, ptInt2) ;
|
|
if ( nLineTriaIndex != ILTT_NO && nLineTriaIndex != ILTT_IN)
|
|
return true ;
|
|
}
|
|
}
|
|
// Base maggiore
|
|
Plane3d plMaxBase ;
|
|
plMaxBase.Set( Point3d( 0., 0., dHeight), Z_AX) ;
|
|
int nMaxBaseTriaIndex = IntersPlanePlane( plPlaneTria, plMaxBase, ptLine, vtLine) ;
|
|
if ( nMaxBaseTriaIndex == IPPT_OVERLAPS) {
|
|
if ( CoplanarDiscTriangleInterference( plMaxBase.GetPoint(), dMaxRad, trMyTria, TriangleType::CLOSED))
|
|
return true ;
|
|
return false ;
|
|
}
|
|
else if ( nMaxBaseTriaIndex == IPPT_YES) {
|
|
double dU1, dU2 ;
|
|
if ( LineDisc( ptLine, vtLine, Point3d( 0., 0., dHeight), Z_AX, dMaxRad, dU1, dU2) == D_INFINITE_INT_LINE_ON_PLANE) {
|
|
Point3d ptInt, ptInt2 ;
|
|
int nLineTriaIndex = IntersLineTria( ptLine + dU1 * vtLine, ptLine + dU2 * vtLine, trMyTria, ptInt, ptInt2) ;
|
|
if ( nLineTriaIndex != ILTT_NO && nLineTriaIndex != ILTT_IN)
|
|
return true ;
|
|
}
|
|
}
|
|
// Triangolo tangente al tronco di cono
|
|
Vector3d vtTriaNormXY( vtN.x, vtN.y, 0.) ;
|
|
if ( ! vtTriaNormXY.Normalize())
|
|
return false ;
|
|
Point3d ptContactSt( dMinRad * vtTriaNormXY.x, dMinRad * vtTriaNormXY.y, 0.) ;
|
|
Point3d ptContactEn( dMaxRad * vtTriaNormXY.x, dMaxRad * vtTriaNormXY.y, dHeight) ;
|
|
Point3d ptInt, ptInt2 ;
|
|
int nContactIndex = IntersLineTria( ptContactSt, ptContactEn, trMyTria, ptInt, ptInt2) ;
|
|
return ! ( nContactIndex == ILTT_NO || nContactIndex == ILTT_IN) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
// Il sistema di riferimento deve avere l'asse di simmetria del cono come asse Z e origine nel centro della base.
|
|
// La distanza di sicurezza ha effetto solo se maggiore di epsilon, altrimenti è ignorata ed è ininfluente.
|
|
bool
|
|
CDeConeFrustumTria( const Frame3d& frCone, double dBaseRad, double dTopRad, double dHeight,
|
|
const Triangle3d& trTria, double dSafeDist)
|
|
{
|
|
// Se il tronco di cono o il triangolo non sono ben definiti non procedo
|
|
if ( max( dBaseRad, dTopRad) < EPS_SMALL || dHeight < EPS_SMALL || ! trTria.IsValid())
|
|
return false ;
|
|
|
|
// Se è un cilindro chiamo la routine apposita.
|
|
if ( abs( dBaseRad - dTopRad) < EPS_SMALL)
|
|
return CDeCylTria( frCone, max( dBaseRad, dTopRad), dHeight, trTria, dSafeDist) ;
|
|
|
|
// Se il raggio di base è maggiore del raggio top cambio il sistema di riferimento.
|
|
double dMinRad = dBaseRad ;
|
|
double dMaxRad = dTopRad ;
|
|
Frame3d frMyCone = frCone ;
|
|
if ( dBaseRad > dTopRad) {
|
|
swap( dMinRad, dMaxRad) ;
|
|
frMyCone.Translate( dHeight * frMyCone.VersZ()) ;
|
|
frMyCone.Rotate( frMyCone.Orig(), frMyCone.VersX(), -1, 0) ;
|
|
}
|
|
|
|
// Se è un cono, chiamo la routine apposita
|
|
if ( dMinRad < EPS_SMALL)
|
|
return CDeConeTria( frMyCone, dMaxRad, dHeight, trTria, dSafeDist) ;
|
|
|
|
// Se distanza di sicurezza nulla
|
|
if ( dSafeDist < EPS_SMALL)
|
|
return CDeSimpleConeFrustumTria( frMyCone, dBaseRad, dTopRad, dHeight, trTria) ;
|
|
|
|
// Verifica preliminare con tronco di cono esteso
|
|
double dDiffRad = dMaxRad - dMinRad ;
|
|
double dDeltaVert = dSafeDist * sqrt( 1 + ( dHeight * dHeight / ( dDiffRad * dDiffRad))) ;
|
|
double dDeltaMaxRad = ( dDeltaVert + dSafeDist) * dDiffRad / dHeight ;
|
|
double dDeltaMinRad = ( dDeltaVert - dSafeDist) * dDiffRad / dHeight ;
|
|
Frame3d frTmp = frMyCone ; frTmp.Translate( - dSafeDist * frMyCone.VersZ()) ;
|
|
if ( ! CDeSimpleConeFrustumTria( frTmp, dMinRad + dDeltaMinRad, dMaxRad + dDeltaMaxRad, dHeight + 2 * dSafeDist, trTria))
|
|
return false ;
|
|
|
|
// Tronco di cono intermedio
|
|
double dHypo = sqrt( dDiffRad * dDiffRad + dHeight * dHeight ) ;
|
|
double dDeltaH = dSafeDist * dDiffRad / dHypo ;
|
|
double dDeltaR = dSafeDist * dHeight / dHypo ;
|
|
frTmp = frMyCone ; frTmp.Translate( -dDeltaH * frMyCone.VersZ()) ;
|
|
if ( CDeSimpleConeFrustumTria( frTmp, dMinRad + dDeltaR, dMaxRad + dDeltaR, dHeight, trTria))
|
|
return true ;
|
|
|
|
// Toro in basso
|
|
if ( CDeSimpleConvexTorusTria( frMyCone, dMinRad, dSafeDist, CT_TOT, trTria))
|
|
return true ;
|
|
|
|
// Toro in alto
|
|
frTmp = frMyCone ; frTmp.Translate( dHeight * frMyCone.VersZ()) ;
|
|
if ( CDeSimpleConvexTorusTria( frTmp, dMaxRad, dSafeDist, CT_TOT, trTria))
|
|
return true ;
|
|
|
|
return false ;
|
|
}
|