Egalware/EgtGeomKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

EgtGeomKernel :

Browse Source 
- correzioni nella verifica di collisione di Poliedri con Poliedri (superfici trimesh chiuse) o TriDexel (Zmap).
This commit is contained in:
DarioS
2023-06-19 07:59:05 +02:00
parent 0c1ad4dd86
commit c4f1a7f97b
3 changed files with 86 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files
VolZmapCalculus.cpp
+58 -150
View File
@@ -430,7 +430,7 @@ VolZmap::AvoidSimpleBox( const Frame3d& frBox, const Vector3d& vtDiag, bool bPre
return true ;
// Se verifico solo prima mappa
if ( ! bPrecise) {
if ( ! bPrecise || m_nMapNum == 1) {
// Limiti su indici
int nStI = Clamp( int( b3Int.GetMin().x / m_dStep), 0, m_nNx[0] - 1) ;
@@ -655,7 +655,7 @@ VolZmap::AvoidSimpleSphere( const Point3d& ptCenter, double dRad, bool bPrecise)
return true ;
// Se verifico solo prima mappa
if ( ! bPrecise) {
if ( ! bPrecise || m_nMapNum == 1) {
// Limiti su indici
int nStI = Clamp( int( b3Int.GetMin().x / m_dStep), 0, m_nNx[0] - 1) ;
@@ -820,7 +820,7 @@ VolZmap::AvoidSimpleCylinder( const Frame3d& frCyl, double dR, double dH, bool b
return true ;
// Se verifico solo prima mappa
if ( ! bPrecise) {
if ( ! bPrecise || m_nMapNum == 1) {
// Limiti su indici
int nStI = Clamp( int( b3Int.GetMin().x / m_dStep), 0, m_nNx[0] - 1) ;
@@ -973,101 +973,6 @@ VolZmap::AvoidCylinder( const Frame3d& frCyl, double dR, double dH, double dSafe
return true ;
}
//----------------------------------------------------------------------------
bool
VolZmap::SingleMapDexelConeCollision( int nStI, int nEnI, int nStJ, int nEnJ, const Point3d& ptRefPoint, const Vector3d& vtRefAx,
double dMinRad, double dMaxRad, double dHeight, double dMinBoxH, double dMaxBoxH) const
{
// Ciclo di intersezione dei dexel con il cono (nel riferimento intrinseco)
for ( int i = nStI ; i <= nEnI ; ++ i) {
for ( int j = nStJ ; j <= nEnJ ; ++ j) {
int nPos = j * m_nNx[0] + i ;
int nSize = int( m_Values[0][nPos].size()) ;
if ( nSize == 0)
continue ;
double dParMin = m_Values[0][nPos][0].dMin ;
double dParMax = m_Values[0][nPos][nSize-1].dMax ;
if ( dParMax < dMinBoxH || dParMin > dMaxBoxH)
continue ;
for ( int k = 0 ; k < 5 ; ++ k) {
// se richiesta interruzione, esco
if ( m_bBreak)
return false ;
// Calcolo spillone
Point3d ptLineSt = ORIG + ( i + 0.5) * m_dStep * X_AX + ( j + 0.5) * m_dStep * Y_AX ;
switch ( k) {
case 0 : break ;
case 1 : ptLineSt += -0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ; break ;
case 2 : ptLineSt += +0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ; break ;
case 3 : ptLineSt += +0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; break ;
case 4 : ptLineSt += -0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ; break ;
}
// Cono proprio
if ( dMinRad < EPS_SMALL) {
Point3d ptSegSt = ptLineSt + dParMin * Z_AX ;
double dU1, dU2 ;
int nIntType = SegmentCone( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint, vtRefAx,
dMaxRad, dHeight, dU1, dU2) ;
if ( nIntType == LinCompCCIntersType::CC_ERROR_INT)
return true ;
else if ( nIntType != LinCompCCIntersType::CC_NO_INTERS) {
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2)
return true ;
}
}
nIntType = SegmentDisc( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint + dHeight * vtRefAx,
vtRefAx, dMaxRad, dU1, dU2) ;
if ( nIntType == LinCompDiscIntersType::D_ERROR_INT)
return true ;
else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) {
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2)
return true ;
}
}
}
// Tronco di cono
else {
Point3d ptSegSt = ptLineSt + dParMin * Z_AX ;
double dU1, dU2 ;
int nIntType = SegmentConeFrustum( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint, vtRefAx,
dMinRad, dMaxRad, dHeight, dU1, dU2) ;
if ( nIntType == LinCompCCIntersType::CC_ERROR_INT)
return true ;
else if ( nIntType != LinCompCCIntersType::CC_NO_INTERS) {
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2)
return true ;
}
}
nIntType = SegmentDisc( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint + dHeight * vtRefAx,
vtRefAx, dMaxRad, dU1, dU2) ;
if ( nIntType == LinCompDiscIntersType::D_ERROR_INT)
return true ;
else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) {
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2)
return true ;
}
}
nIntType = SegmentDisc( ptSegSt, Z_AX, dParMax - dParMin, ptRefPoint, vtRefAx, dMinRad, dU1, dU2) ;
if ( nIntType == LinCompDiscIntersType::D_ERROR_INT)
return true ;
else if ( nIntType != LinCompDiscIntersType::D_NO_INTERS) {
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
if ( m_Values[0][nPos][nIndex].dMax >= dU1 && m_Values[0][nPos][nIndex].dMin <= dU2)
return true ;
}
}
}
}
}
}
return false ;
}
//----------------------------------------------------------------------------
bool
VolZmap::AvoidSimpleConeFrustum( const Frame3d& frCone, double dMinRad, double dMaxRad, double dHeight, bool bPrecise) const
@@ -1116,7 +1021,7 @@ VolZmap::AvoidSimpleConeFrustum( const Frame3d& frCone, double dMinRad, double d
return true ;
// Uso solo la prima mappa
if ( ! bPrecise) {
if ( ! bPrecise || m_nMapNum == 1) {
// Limiti su indici
int nStI = Clamp( int( b3Int.GetMin().x / m_dStep), 0, m_nNx[0] - 1) ;
int nEnI = Clamp( int( b3Int.GetMax().x / m_dStep), 0, m_nNx[0] - 1) ;
@@ -1507,7 +1412,7 @@ VolZmap::AvoidSimpleRectPrismoid( const Frame3d& frPrismoid, double dLenghtBaseX
return true ;
// Se verifico solo prima mappa
if ( ! bPrecise) {
if ( ! bPrecise || m_nMapNum == 1) {
// Limiti su indici
int nStI = Clamp( int( b3Int.GetMin().x / m_dStep), 0, m_nNx[0] - 1) ;
int nEnI = Clamp( int( b3Int.GetMax().x / m_dStep), 0, m_nNx[0] - 1) ;
@@ -1785,7 +1690,7 @@ VolZmap::AvoidSimpleTorus( const Frame3d& frTorus, double dMaxRad, double dMinRa
return true ;
// Se verifico solo prima mappa
if ( ! bPrecise) {
if ( ! bPrecise || m_nMapNum == 1) {
// Limiti su indici
int nStI = Clamp( int( b3Int.GetMin().x / m_dStep), 0, m_nNx[0] - 1) ;
int nEnI = Clamp( int( b3Int.GetMax().x / m_dStep), 0, m_nNx[0] - 1) ;
@@ -1929,45 +1834,50 @@ VolZmap::AvoidTorus( const Frame3d& frTorus, double dMaxRad, double dMinRad,
bool
VolZmap::AvoidSurfTm( const ISurfTriMesh& tmSurf, double dSafeDist, bool bPrecise) const
{
// Controllo sulla validità della superficie ed eventualmente sulla sua chiusura
// Controllo sulla validità della superficie ed eventualmente sulla sua chiusura
if ( ! ( tmSurf.IsValid() && tmSurf.IsClosed()))
return false ;
// Bounding box della superficie espresso nel sistema locale
// Bounding box della superficie espresso nel sistema locale
BBox3d b3SurfBox ;
tmSurf.GetLocalBBox( b3SurfBox) ;
// Se la distanza di sicurezza è significativa, espando il box
if ( dSafeDist > EPS_SMALL )
b3SurfBox.Expand( dSafeDist) ;
// Box dello Zmap nel suo sistema locale
// Box dello Zmap nel suo sistema locale
BBox3d b3ZmapBox( ORIG, Point3d( m_nNx[0] * m_dStep, m_nNy[0] * m_dStep, m_dMaxZ[0])) ;
b3ZmapBox.ToGlob( m_MapFrame) ;
// Box intersezione: se non c'è intersezione ho finito.
// Se la distanza di sicurezza è significativa, espando il box
if ( dSafeDist > EPS_SMALL )
b3ZmapBox.Expand( dSafeDist) ;
// Box intersezione: se non c'è intersezione ho finito.
BBox3d b3IntBox ;
if ( ! b3ZmapBox.FindIntersection( b3SurfBox, b3IntBox))
return true ;
// Recupero i triangoli della superficie che cadono nel box intersezione.
// Recupero i triangoli della superficie che cadono nel box intersezione.
INTVECTOR vTriaIndex ;
tmSurf.GetAllTriaOverlapBox( b3IntBox, vTriaIndex) ;
// Non è richeista precisione
if ( ! bPrecise) {
// Ciclo sui triangoli che cadono nel box
// Se verifico solo prima mappa
if ( ! bPrecise || m_nMapNum == 1) {
// Ciclo sui triangoli che cadono nel box
for ( int nT : vTriaIndex) {
Triangle3d trTria ;
if ( ! ( tmSurf.GetTriangle( nT, trTria) && trTria.Validate()))
continue ;
BBox3d b3TriaBox ;
trTria.GetLocalBBox( b3TriaBox) ;
// Se è necessario, espando il box di una costante additiva pari alla distanza di sicurezza.
// Se è necessario, espando il box di una costante additiva pari alla distanza di sicurezza.
if ( dSafeDist > EPS_SMALL)
b3TriaBox.Expand( dSafeDist) ;
// Porto il bounding-box del triangolo nel sistema intrinseco.
// Copia del triangolo con eventuale traslazione
Triangle3d trNewTria = trTria ;
if ( dSafeDist > EPS_SMALL)
trNewTria.Translate( dSafeDist * trTria.GetN()) ;
// Porto il bounding-box del triangolo nel sistema intrinseco.
b3TriaBox.ToLoc( m_MapFrame) ;
// Limiti su indici
// Limiti su indici
int nStI = Clamp( int( b3TriaBox.GetMin().x / m_dStep), 0, m_nNx[0] - 1) ;
int nEnI = Clamp( int( b3TriaBox.GetMax().x / m_dStep), 0, m_nNx[0] - 1) ;
int nStJ = Clamp( int( b3TriaBox.GetMin().y / m_dStep), 0, m_nNy[0] - 1) ;
int nEnJ = Clamp( int( b3TriaBox.GetMax().y / m_dStep), 0, m_nNy[0] - 1) ;
// Ciclo di intersezione dei dexel con il toro (nel riferimento intrinseco)
// Ciclo di intersezione dei dexel con il triangolo (nel riferimento intrinseco)
for ( int i = nStI ; i <= nEnI ; ++ i) {
for ( int j = nStJ ; j <= nEnJ ; ++ j) {
int nPos = j * m_nNx[0] + i ;
@@ -1987,21 +1897,19 @@ VolZmap::AvoidSurfTm( const ISurfTriMesh& tmSurf, double dSafeDist, bool bPrecis
else if ( k == 4)
ptLineSt += - 0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
Vector3d vtLineDir = Z_AX ;
// Porto punto iniziale e vettore nel sistema locale
// Porto punto iniziale e vettore nel sistema locale
ptLineSt.ToGlob( m_MapFrame) ;
vtLineDir.ToGlob( m_MapFrame) ;
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
// Segmento ormai nel sistema locale
// Segmento ormai nel sistema locale
Point3d ptSegSt = ptLineSt + m_Values[0][nPos][nIndex].dMin * vtLineDir ;
double dSegLen = m_Values[0][nPos][nIndex].dMax - m_Values[0][nPos][nIndex].dMin ;
// Copia del triangolo che può essere evenutalmente traslata
Triangle3d trNewTria = trTria ;
// Se la distanza di sicurezza è significativa
// Se la distanza di sicurezza è significativa
if ( dSafeDist > EPS_SMALL) {
// Valuto sfere nei vertici e cilindri lungo gli edge.
// Valuto sfere nei vertici e cilindri lungo gli edge.
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Point3d ptVertP = trTria.GetP( nV) ;
Vector3d vtEdgeV = trTria.GetP( ( nV + 1)) - ptVertP ;
Vector3d vtEdgeV = trTria.GetP( ( nV + 1) % 3) - ptVertP ;
double dEdgeLen = vtEdgeV.Len() ;
vtEdgeV /= dEdgeLen ;
double dU1, dU2 ;
@@ -2010,16 +1918,14 @@ VolZmap::AvoidSurfTm( const ISurfTriMesh& tmSurf, double dSafeDist, bool bPrecis
return false ;
nIntersType = IntersSegmentCylinder( ptSegSt, vtLineDir, dSegLen, ptVertP, vtEdgeV,
dSafeDist, dEdgeLen, dU1, dU2) ;
if ( nIntersType != LinCompCCIntersType::CC_NO_INTERS )
if ( nIntersType != LinCompCCIntersType::CC_NO_INTERS)
return false ;
}
// Traslo il triangolo.
trNewTria.Translate( dSafeDist * trTria.GetN()) ;
}
// Intersezione segento triangolo
// Intersezione segmento con triangolo eventualmente offsettato
Point3d ptInt, ptInt2 ;
int nIntersType = IntersLineTria( ptSegSt, vtLineDir, dSegLen, trNewTria, ptInt, ptInt2) ;
// Collisione
// Collisione
if ( nIntersType != IntLineTriaType::ILTT_NO)
return false ;
}
@@ -2028,25 +1934,26 @@ VolZmap::AvoidSurfTm( const ISurfTriMesh& tmSurf, double dSafeDist, bool bPrecis
}
}
}
// È richiesta precisione
// altrimenti verifico con tutte e tre le mappe
else {
// Ciclo sui triangoli che cadono nel box
// Ciclo sui triangoli che cadono nel box
for ( int nT : vTriaIndex) {
Triangle3d trTria ;
if ( ! ( tmSurf.GetTriangle( nT, trTria) && trTria.Validate()))
continue ;
BBox3d b3TriaBox ;
trTria.GetLocalBBox( b3TriaBox) ;
// Se è necessario, espando il box di una costante additiva pari alla distanza di sicurezza.
// Se è necessario, espando il box di una costante additiva pari alla distanza di sicurezza.
if ( dSafeDist > EPS_SMALL)
b3TriaBox.Expand( dSafeDist) ;
// Porto il bounding-box del triangolo nel sistema intrinseco.
// Porto il bounding-box del triangolo nel sistema intrinseco.
b3TriaBox.ToLoc( m_MapFrame) ;
// Ciclo sulle mappe
// Ciclo sulle mappe
for ( int nMap = 0 ; nMap < m_nMapNum ; ++ nMap) {
Point3d ptInfIntBox = b3TriaBox.GetMin() ;
Point3d ptSupIntBox = b3TriaBox.GetMax() ;
// Dal sistema intrinseco al sistema griglia (per la prima griglia coincidono).
// Dal sistema intrinseco al sistema griglia (per la prima griglia coincidono).
if ( nMap == 1) {
swap( ptInfIntBox.x, ptInfIntBox.z) ;
swap( ptInfIntBox.x, ptInfIntBox.y) ;
@@ -2059,24 +1966,24 @@ VolZmap::AvoidSurfTm( const ISurfTriMesh& tmSurf, double dSafeDist, bool bPrecis
swap( ptSupIntBox.y, ptSupIntBox.z) ;
swap( ptSupIntBox.x, ptSupIntBox.y) ;
}
// Limiti su indici
// Limiti su indici
int nStI = Clamp( int( ptInfIntBox.x / m_dStep), 0, m_nNx[nMap] - 1) ;
int nEnI = Clamp( int( ptSupIntBox.x / m_dStep), 0, m_nNx[nMap] - 1) ;
int nStJ = Clamp( int( ptInfIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ;
int nEnJ = Clamp( int( ptSupIntBox.y / m_dStep), 0, m_nNy[nMap] - 1) ;
for ( int nDex = 0 ; nDex < int( m_Values[nMap].size()) ; ++ nDex) {
// Indici del dexel
// Indici del dexel
int nI = nDex % m_nNx[nMap] ;
int nJ = nDex / m_nNx[nMap] ;
// Se fuori dalla regione ammissibile salto l'iterazione
// Se fuori dalla regione ammissibile salto l'iterazione
if ( nI < nStI || nI > nEnI || nJ < nStJ || nJ > nEnJ)
continue ;
// Posizione del dexel
// Posizione del dexel
double dX = ( nI + 0.5) * m_dStep ;
double dY = ( nJ + 0.5) * m_dStep ;
Point3d ptLineSt( dX, dY, 0.) ;
Vector3d vtLineDir( 0., 0., 1.) ;
// Dal sistema griglia al sistema intrinseco (per la prima griglia coincidono).
// Dal sistema griglia al sistema intrinseco (per la prima griglia coincidono).
if ( nMap == 1) {
swap( ptLineSt.x, ptLineSt.y) ;
swap( ptLineSt.x, ptLineSt.z) ;
@@ -2089,22 +1996,22 @@ VolZmap::AvoidSurfTm( const ISurfTriMesh& tmSurf, double dSafeDist, bool bPrecis
swap( vtLineDir.x, vtLineDir.y) ;
swap( vtLineDir.y, vtLineDir.z) ;
}
// Porto punto iniziale e vettore nel sistema locale
// Porto punto iniziale e vettore nel sistema locale
ptLineSt.ToGlob( m_MapFrame) ;
vtLineDir.ToGlob( m_MapFrame) ;
// Ciclo sui segmenti del dexel.
// Ciclo sui segmenti del dexel.
for ( int nInt = 0 ; nInt < int( m_Values[nMap][nDex].size()) ; ++ nInt) {
// Segmento ormai nel sistema locale
// Segmento ormai nel sistema locale
Point3d ptSegSt = ptLineSt + m_Values[nMap][nDex][nInt].dMin * vtLineDir ;
double dSegLen = m_Values[nMap][nDex][nInt].dMax - m_Values[nMap][nDex][nInt].dMin ;
// Copia del triangolo che può essere evenutalmente traslata
// Copia del triangolo che può essere eventualmente traslata
Triangle3d trNewTria = trTria ;
// Se la distanza di sicurezza è significativa
// Se la distanza di sicurezza è significativa
if ( dSafeDist > EPS_SMALL) {
// Valuto sfere nei vertici e cilindri lungo gli edge.
// Valuto sfere nei vertici e cilindri lungo gli edge.
for ( int nV = 0 ; nV < 3 ; ++ nV) {
Point3d ptVertP = trTria.GetP( nV) ;
Vector3d vtEdgeV = trTria.GetP( ( nV + 1)) - ptVertP ;
Vector3d vtEdgeV = trTria.GetP( ( nV + 1) % 3) - ptVertP ;
double dEdgeLen = vtEdgeV.Len() ;
vtEdgeV /= dEdgeLen ;
double dU1, dU2 ;
@@ -2113,16 +2020,16 @@ VolZmap::AvoidSurfTm( const ISurfTriMesh& tmSurf, double dSafeDist, bool bPrecis
return false ;
nIntersType = IntersSegmentCylinder( ptSegSt, vtLineDir, dSegLen, ptVertP, vtEdgeV,
dSafeDist, dEdgeLen, dU1, dU2) ;
if ( nIntersType != LinCompCCIntersType::CC_NO_INTERS )
if ( nIntersType != LinCompCCIntersType::CC_NO_INTERS)
return false ;
}
// Traslo il triangolo.
// Traslo il triangolo.
trNewTria.Translate( dSafeDist * trTria.GetN()) ;
}
// Intersezione segento triangolo
// Intersezione segmento triangolo
Point3d ptInt, ptInt2 ;
int nIntersType = IntersLineTria( ptSegSt, vtLineDir, dSegLen, trNewTria, ptInt, ptInt2) ;
// Collisione
// Collisione
if ( nIntersType != IntLineTriaType::ILTT_NO)
return false ;
}
@@ -2130,6 +2037,7 @@ VolZmap::AvoidSurfTm( const ISurfTriMesh& tmSurf, double dSafeDist, bool bPrecis
}
}
}
return true ;
}