Egalware/EgtGeomKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
ba7379e752d5b0bfc97f7f6a1470951b417325b7
EgtGeomKernel/CDeRectPrismoidTria.cpp
T
Dario Sassi ddade325c4 EgtGeomKernel 2.6b3 : 
- 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).
2024-02-16 08:43:15 +01:00

235 lines
11 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 "CDeCapsTria.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,
const Triangle3d& trTria, double dSafeDist)
{
// 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 ;
// Porto il triangolo nel riferimento del prismoide
Triangle3d trTriaL = trTria ;
trTriaL.ToLoc( frPrismoid) ;
// Se distanza di sicurezza nulla
if ( dSafeDist < EPS_SMALL)
return CDeSimpleRectPrismoidTria( Frame3d(), dLenghtBaseX, dLenghtBaseY, dLenghtTopX, dLenghtTopY, dHeight, trTriaL) ;
// 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 frTmp( Point3d( 0, 0, -dSafeDist)) ;
if ( ! CDeSimpleRectPrismoidTria( frTmp, dLenghtBaseX + 2 * dOffsBaseX, dLenghtBaseY + 2 * dOffsBaseY,
dLenghtTopX + 2 * dOffsTopX, dLenghtTopY + 2 * dOffsTopY, dHeight + 2 * dSafeDist, trTriaL))
return false ;
// Offset fine
// Box sotto
frTmp.ChangeOrig( Point3d( 0, 0, -dSafeDist)) ;
if ( CDeSimpleBoxTria( frTmp, Vector3d( dLenghtBaseX, dLenghtBaseY, dSafeDist), trTriaL))
return true ;
// Box sopra
frTmp.ChangeOrig( Point3d( 0, 0, dHeight)) ;
if ( CDeSimpleBoxTria( frTmp, Vector3d( dLenghtTopX, dLenghtTopY, dSafeDist), trTriaL))
return true ;
// Prismoide allungato in X
double dHypoX = sqrt( dHDiffX * dHDiffX + dHeight * dHeight) ;
double dOffsX = dSafeDist * dHeight / dHypoX ;
double dMoveXZ = dSafeDist * dHDiffX / dHypoX ;
frTmp.ChangeOrig( Point3d( 0, 0, dMoveXZ)) ;
if ( CDeSimpleRectPrismoidTria( frTmp, dLenghtBaseX + 2 * dOffsX, dLenghtBaseY,
dLenghtTopX + 2 * dOffsX, dLenghtTopY, dHeight, trTriaL))
return true ;
// Prismoide allungato in Y
double dHypoY = sqrt( dHDiffY * dHDiffY + dHeight * dHeight) ;
double dOffsY = dSafeDist * dHeight / dHypoY ;
double dMoveYZ = dSafeDist * dHDiffY / dHypoY ;
frTmp.ChangeOrig( Point3d( 0, 0, dMoveYZ)) ;
if ( CDeSimpleRectPrismoidTria( frTmp, dLenghtBaseX, dLenghtBaseY + 2 * dOffsY,
dLenghtTopX, dLenghtTopY + 2 * dOffsY, dHeight, trTriaL))
return true ;
// 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)} ;
// Capsule centrati sui dodici spigoli
if ( CDeSimpleCapsTria( vVert[0], vVert[1], dSafeDist, trTriaL))
return true ;
if ( CDeSimpleCapsTria( vVert[1], vVert[2], dSafeDist, trTriaL))
return true ;
if ( CDeSimpleCapsTria( vVert[2], vVert[3], dSafeDist, trTriaL))
return true ;
if ( CDeSimpleCapsTria( vVert[3], vVert[0], dSafeDist, trTriaL))
return true ;
if ( CDeSimpleCapsTria( vVert[4], vVert[5], dSafeDist, trTriaL))
return true ;
if ( CDeSimpleCapsTria( vVert[5], vVert[6], dSafeDist, trTriaL))
return true ;
if ( CDeSimpleCapsTria( vVert[6], vVert[7], dSafeDist, trTriaL))
return true ;
if ( CDeSimpleCapsTria( vVert[7], vVert[4], dSafeDist, trTriaL))
return true ;
if ( CDeSimpleCapsTria( vVert[0], vVert[4], dSafeDist, trTriaL))
return true ;
if ( CDeSimpleCapsTria( vVert[1], vVert[5], dSafeDist, trTriaL))
return true ;
if ( CDeSimpleCapsTria( vVert[2], vVert[6], dSafeDist, trTriaL))
return true ;
if ( CDeSimpleCapsTria( vVert[3], vVert[7], dSafeDist, trTriaL))
return true ;
return false ;
}
Reference in New Issue View Git Blame Copy Permalink