Egalware/EgtGeomKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
bd80f16812b79ff614fdc1f4e2dbf514e1ff54ea
EgtGeomKernel/CDeRectPrismoidTria.cpp
T
DarioS 73f5b382c9 EgtGeomKernel 2.3g1 : 
- versione x64 compilata con Clang-cl/LLVM
- modifiche varie per eliminare warning più gravi di questo compilatore.
2021-07-20 12:53:04 +02:00

255 lines
12 KiB
C++
Raw Blame History

//----------------------------------------------------------------------------
// EgalTech 2020-2020
//----------------------------------------------------------------------------
// File : CDeRectPrismoidTria.cpp Data : 29.10.20 Versione : 2.2k1
// Contenuto : Implementazione della verifica di collisione tra
// Prismoide a basi rettangolari e Triangle3d.
//
//
// Modifiche : 29.10.20 LM Creazione modulo.
//
//
//----------------------------------------------------------------------------
//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "CDeRectPrismoidTria.h"
#include "CDeBoxTria.h"
#include "CDeCylTria.h"
#include "CDeSpheTria.h"
#include "/EgtDev/Include/EGkIntersTriaTria.h"
using namespace std ;
//----------------------------------------------------------------------------
// Il sistema di riferimento deve avere l'origine nel centro della Base,
// asse X lungo un lato della stessa e asse Z ortogonale alle basi e diretta verso la base Top.
bool
CDeSimpleRectPrismoidTria( const Frame3d& frPrismoid, double dLenghtBaseX, double dLenghtBaseY,
double dLenghtTopX, double dLenghtTopY, double dHeight, const Triangle3d& trTria)
{
// Porto il triangolo nel sistema di riferimento del tronco di cono
Triangle3d trMyTria = trTria ;
trMyTria.ToLoc( frPrismoid) ;
double dHalfBaseX = 0.5 * dLenghtBaseX;
double dHalfBaseY = 0.5 * dLenghtBaseY ;
double dHalfTopX = 0.5 * dLenghtTopX ;
double dHalfTopY = 0.5 * dLenghtTopY ;
// Se almeno un vertice collide ho finito
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Point3d ptVert = trMyTria.GetP( nV) ;
if ( ptVert.z > - EPS_SMALL && ptVert.z < dHeight + EPS_SMALL &&
( ptVert.x + dHalfBaseX + EPS_SMALL) * dHeight > ( dHalfBaseX - dHalfTopX) * ptVert.z &&
( ptVert.x - dHalfBaseX - EPS_SMALL) * dHeight < ( dHalfTopX - dHalfBaseX) * ptVert.z &&
( ptVert.y + dHalfBaseY + EPS_SMALL) * dHeight > ( dHalfBaseY - dHalfTopY) * ptVert.z &&
( ptVert.y - dHalfBaseY - EPS_SMALL) * dHeight < ( dHalfTopY - dHalfBaseY) * ptVert.z)
return true ;
}
// Se c'è collisione con almeno un triangolo delle facce ho finito
Triangle3d trFaceTria1, trFaceTria2 ;
Point3d ptInt, ptIn2 ;
TRIA3DVECTOR vTria ;
// Faccia base
trFaceTria1.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.),
Point3d( - dHalfBaseX, dHalfBaseY, 0.),
Point3d( dHalfBaseX, dHalfBaseY, 0.)) ;
trFaceTria1.Validate() ;
trFaceTria2.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.),
Point3d( dHalfBaseX, dHalfBaseY, 0.),
Point3d( dHalfBaseX, - dHalfBaseY, 0.)) ;
trFaceTria2.Validate() ;
if ( IntersTriaTria( trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO ||
IntersTriaTria( trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO)
return true ;
// Faccia top
trFaceTria1.Set( Point3d( - dHalfTopX, - dHalfTopY, dHeight),
Point3d( dHalfTopX, dHalfTopY, dHeight),
Point3d( - dHalfTopX, dHalfTopY, dHeight)) ;
trFaceTria1.Validate() ;
trFaceTria2.Set( Point3d( - dHalfTopX, - dHalfTopY, dHeight),
Point3d( dHalfTopX, - dHalfTopY, dHeight),
Point3d( dHalfTopX, dHalfTopY, dHeight)) ;
trFaceTria2.Validate() ;
if ( IntersTriaTria( trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO ||
IntersTriaTria( trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO)
return true ;
// Faccia laterale 1
trFaceTria1.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.),
Point3d( dHalfTopX , - dHalfTopY , dHeight),
Point3d( - dHalfTopX , - dHalfTopY , dHeight)) ;
trFaceTria1.Validate() ;
trFaceTria2.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.),
Point3d( dHalfBaseX, - dHalfBaseY, 0.),
Point3d( dHalfTopX, - dHalfTopY , dHeight)) ;
trFaceTria2.Validate() ;
if ( IntersTriaTria( trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO ||
IntersTriaTria( trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO)
return true ;
// Faccia laterale 2
trFaceTria1.Set( Point3d( dHalfBaseX, - dHalfBaseY, 0.),
Point3d( dHalfTopX , dHalfTopY , dHeight),
Point3d( dHalfTopX , - dHalfTopY , dHeight)) ;
trFaceTria1.Validate() ;
trFaceTria2.Set( Point3d( dHalfBaseX, - dHalfBaseY, 0.),
Point3d( dHalfBaseX, dHalfBaseY, 0.),
Point3d( dHalfTopX , dHalfTopY , dHeight)) ;
trFaceTria2.Validate() ;
if ( IntersTriaTria(trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO ||
IntersTriaTria(trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO)
return true ;
// Faccia laterale 3
trFaceTria1.Set( Point3d( dHalfBaseX, dHalfBaseY, 0.),
Point3d( - dHalfTopX , dHalfTopY , dHeight),
Point3d( dHalfTopX , dHalfTopY , dHeight)) ;
trFaceTria1.Validate() ;
trFaceTria2.Set( Point3d( dHalfBaseX, dHalfBaseY, 0.),
Point3d( - dHalfBaseX, dHalfBaseY, 0.),
Point3d( - dHalfTopX , dHalfTopY , dHeight)) ;
trFaceTria2.Validate() ;
if ( IntersTriaTria( trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO ||
IntersTriaTria( trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO)
return true ;
// Faccia laterale 4
trFaceTria1.Set( Point3d( - dHalfBaseX, dHalfBaseY, 0.),
Point3d( - dHalfTopX , - dHalfTopY , dHeight),
Point3d( - dHalfTopX , dHalfTopY , dHeight)) ;
trFaceTria1.Validate() ;
trFaceTria2.Set( Point3d( - dHalfBaseX, dHalfBaseY, 0.),
Point3d( - dHalfBaseX, - dHalfBaseY, 0.),
Point3d( - dHalfTopX , - dHalfTopY , dHeight)) ;
trFaceTria2.Validate() ;
if ( IntersTriaTria( trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO ||
IntersTriaTria( trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO)
return true ;
return false ;
}
//----------------------------------------------------------------------------
// Il sistema di riferimento deve avere l'origine nel centro della base Base, asse X lungo
// un segmento della stessa e asse Z ortogonale alle basi e diretta verso la base Top.
bool
CDeRectPrismoidTria( const Frame3d& frPrismoid, double dLenghtBaseX, double dLenghtBaseY,
double dLenghtTopX, double dLenghtTopY, double dHeight,
double dSafeDist, const Triangle3d& trTria)
{
// Se il tronco di piramide o il triangolo non sono ben definiti non procedo
if ( max( dLenghtBaseX, dLenghtTopX) < EPS_SMALL ||
max( dLenghtBaseY, dLenghtTopY) < EPS_SMALL ||
dHeight < EPS_SMALL || ! trTria.IsValid())
return false ;
// Se distanza di sicurezza nulla
if ( dSafeDist < EPS_SMALL)
return CDeSimpleRectPrismoidTria( frPrismoid, dLenghtBaseX, dLenghtBaseY, dLenghtTopX, dLenghtTopY, dHeight, trTria) ;
// Verifiche con offset esteso
double dHDiffX = ( dLenghtBaseX - dLenghtTopX) / 2 ;
double dTgAx = dHDiffX / dHeight ;
double dSecAx = sqrt( 1 + dTgAx * dTgAx) ;
double dOffsBaseX = dSafeDist * ( dSecAx + dTgAx) ;
double dOffsTopX = dSafeDist * ( dSecAx - dTgAx) ;
double dHDiffY = ( dLenghtBaseY - dLenghtTopY) / 2 ;
double dTgAy = dHDiffY / dHeight ;
double dSecAy = sqrt( 1 + dTgAy * dTgAy) ;
double dOffsBaseY = dSafeDist * ( dSecAy + dTgAy) ;
double dOffsTopY = dSafeDist * ( dSecAy - dTgAy) ;
Frame3d frFrame = frPrismoid ; frFrame.Translate( - dSafeDist * frFrame.VersZ()) ;
if ( ! CDeSimpleRectPrismoidTria( frFrame, dLenghtBaseX + 2 * dOffsBaseX, dLenghtBaseY + 2 * dOffsBaseY,
dLenghtTopX + 2 * dOffsTopX, dLenghtTopY + 2 * dOffsTopY, dHeight + 2 * dSafeDist, trTria))
return false ;
// Offset fine
// Box sotto
frFrame = frPrismoid ; frFrame.Translate( - dSafeDist * frFrame.VersZ()) ;
if ( CDeSimpleBoxTria( frFrame, Vector3d( dLenghtBaseX, dLenghtBaseY, dSafeDist), trTria))
return true ;
// Box sopra
frFrame = frPrismoid ; frFrame.Translate( dHeight * frFrame.VersZ()) ;
if ( CDeSimpleBoxTria( frFrame, Vector3d( dLenghtTopX, dLenghtTopY, dSafeDist), trTria))
return true ;
// Prismoide allungato in X
double dHypoX = sqrt( dHDiffX * dHDiffX + dHeight * dHeight) ;
double dOffsX = dSafeDist * dHeight / dHypoX ;
double dMoveXZ = dSafeDist * dHDiffX / dHypoX ;
frFrame = frPrismoid ; frFrame.Translate( dMoveXZ * frFrame.VersZ()) ;
if ( CDeSimpleRectPrismoidTria( frFrame, dLenghtBaseX + 2 * dOffsX, dLenghtBaseY,
dLenghtTopX + 2 * dOffsX, dLenghtTopY, dHeight, trTria))
return true ;
// Prismoide allungato in Y
double dHypoY = sqrt( dHDiffY * dHDiffY + dHeight * dHeight) ;
double dOffsY = dSafeDist * dHeight / dHypoY ;
double dMoveYZ = dSafeDist * dHDiffY / dHypoY ;
frFrame = frPrismoid ; frFrame.Translate( dMoveYZ * frFrame.VersZ()) ;
if ( CDeSimpleRectPrismoidTria( frFrame, dLenghtBaseX, dLenghtBaseY + 2 * dOffsY,
dLenghtTopX, dLenghtTopY + 2 * dOffsY, dHeight, trTria))
return true ;
// Sfere centrate nei vertici
double dHalfBaseX = dLenghtBaseX / 2 ;
double dHalfBaseY = dLenghtBaseY / 2 ;
double dHalfTopX = dLenghtTopX / 2 ;
double dHalfTopY = dLenghtTopY / 2 ;
PNTVECTOR vVert = { Point3d( -dHalfBaseX, -dHalfBaseY, 0.),
Point3d( dHalfBaseX, -dHalfBaseY, 0.),
Point3d( dHalfBaseX, dHalfBaseY, 0.),
Point3d( -dHalfBaseX, dHalfBaseY, 0.),
Point3d( -dHalfTopX, -dHalfTopY, dHeight),
Point3d( dHalfTopX, -dHalfTopY, dHeight),
Point3d( dHalfTopX, dHalfTopY, dHeight),
Point3d( -dHalfTopX, dHalfTopY, dHeight)} ;
for ( auto& ptV : vVert) {
ptV.ToGlob( frPrismoid) ;
if ( CDeSimpleSpheTria( ptV, dSafeDist, trTria))
return true ;
}
// Cilindri con i segmenti come asse
frFrame.Set( vVert[0], frPrismoid.VersX()) ;
if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtBaseX, trTria))
return true ;
frFrame.Set( vVert[1], frPrismoid.VersY()) ;
if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtBaseY, trTria))
return true ;
frFrame.Set( vVert[2], -frPrismoid.VersX()) ;
if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtBaseX, trTria))
return true ;
frFrame.Set( vVert[3], -frPrismoid.VersY()) ;
if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtBaseY, trTria))
return true ;
frFrame.Set( vVert[4], frPrismoid.VersX()) ;
if (CDeSimpleCylTria( frFrame, dSafeDist, dLenghtTopX, trTria))
return true ;
frFrame.Set( vVert[5], frPrismoid.VersY()) ;
if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtTopY, trTria))
return true ;
frFrame.Set( vVert[6], -frPrismoid.VersX()) ;
if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtTopX, trTria))
return true ;
frFrame.Set( vVert[7], -frPrismoid.VersY()) ;
if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtTopY, trTria))
return true ;
Vector3d vtSeg04 = vVert[4] - vVert[0] ;
double dLenSeg04 = vtSeg04.Len() ;
frFrame.Set( vVert[0], vtSeg04) ;
if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenSeg04, trTria))
return true ;
Vector3d vtSeg15 = vVert[5] - vVert[1] ;
double dLenSeg15 = vtSeg15.Len() ;
frFrame.Set( vVert[1], vtSeg15) ;
if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenSeg15, trTria))
return true ;
Vector3d vtSeg26 = vVert[6] - vVert[2] ;
double dLenSeg26 = vtSeg26.Len() ;
frFrame.Set( vVert[2], vtSeg26) ;
if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenSeg26, trTria))
return true ;
Vector3d vtSeg37 = vVert[7] - vVert[3] ;
double dLenSeg37 = vtSeg37.Len();
frFrame.Set( vVert[3], vtSeg37) ;
if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenSeg37, trTria))
return true ;
return false ;
}
Reference in New Issue View Git Blame Copy Permalink