EgtGeomKernel 2.2j4 :
- aggiunta verifica di collisione di tronco di cono e piramide con Zmap - aggiunto a verifiche di collisione di Zmap con box, cilindro e sfera flag bPrecise (indica di fare i controlli con tutti e tre i sistemi di spilloni dello Zmap).
This commit is contained in:
Dario Sassi
+510
-104
@@ -499,7 +499,7 @@ VolZmap::GetDepthWithVoxel( const Point3d& ptP, const Vector3d& vtD, double& dIn
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
bool
|
||||
VolZmap::AvoidBox( const Frame3d& frBox, const Vector3d& vtDiag, double dSafeDist) const
|
||||
VolZmap::AvoidBox( const Frame3d& frBox, const Vector3d& vtDiag, double dSafeDist, bool bPrecise) const
|
||||
{
|
||||
// BBox
|
||||
BBox3d b3Box( ORIG, ORIG + vtDiag) ;
|
||||
@@ -525,57 +525,66 @@ VolZmap::AvoidBox( const Frame3d& frBox, const Vector3d& vtDiag, double dSafeDis
|
||||
if ( ! b3Zmap.FindIntersection( b3Box, b3Int))
|
||||
return true ;
|
||||
|
||||
// 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) ;
|
||||
int nStJ = Clamp( int( b3Int.GetMin().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
int nEnJ = Clamp( int( b3Int.GetMax().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
// Se verifico solo prima mappa
|
||||
if ( ! bPrecise) {
|
||||
|
||||
// Vettore direzione dei dexel nel riferimento del Box
|
||||
Vector3d vtK = Z_AX ; vtK.LocToLoc( m_MapFrame, frB) ;
|
||||
// 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) ;
|
||||
int nStJ = Clamp( int( b3Int.GetMin().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
int nEnJ = Clamp( int( b3Int.GetMax().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
|
||||
// Riferimento intrinseco dei dexel nel riferimento del box
|
||||
Point3d ptO = ORIG ; ptO.LocToLoc( m_MapFrame, frB) ;
|
||||
Vector3d vtX = X_AX ; vtX.LocToLoc( m_MapFrame, frB) ;
|
||||
Vector3d vtY = Y_AX ; vtY.LocToLoc( m_MapFrame, frB) ;
|
||||
// Vettore direzione dei dexel nel riferimento del Box
|
||||
Vector3d vtK = Z_AX ; vtK.LocToLoc( m_MapFrame, frB) ;
|
||||
|
||||
// Ciclo di intersezione dei dexel con il BBox
|
||||
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 ;
|
||||
for ( int k = 0 ; k < 5 ; ++ k) {
|
||||
Point3d ptT = ptO + ( i + 0.5) * m_dStep * vtX + ( j + 0.5) * m_dStep * vtY ;
|
||||
if ( k == 0)
|
||||
;
|
||||
else if ( k == 1)
|
||||
ptT += - 0.4 * m_dStep * vtX - 0.4 * m_dStep * vtY ;
|
||||
else if ( k == 2)
|
||||
ptT += + 0.4 * m_dStep * vtX - 0.4 * m_dStep * vtY ;
|
||||
else if ( k == 3)
|
||||
ptT += + 0.4 * m_dStep * vtX + 0.4 * m_dStep * vtY ;
|
||||
else if ( k == 4)
|
||||
ptT += - 0.4 * m_dStep * vtX + 0.4 * m_dStep * vtY ;
|
||||
double dZmin, dZmax ;
|
||||
if ( IntersLineBox( ptT, vtK, ORIG, ORIG + vtDg, dZmin, dZmax)) {
|
||||
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
|
||||
if ( ! ( dZmax < m_Values[0][nPos][nIndex].dMin - EPS_SMALL ||
|
||||
dZmin > m_Values[0][nPos][nIndex].dMax + EPS_SMALL))
|
||||
return false ;
|
||||
// Riferimento intrinseco dei dexel nel riferimento del box
|
||||
Point3d ptO = ORIG ; ptO.LocToLoc( m_MapFrame, frB) ;
|
||||
Vector3d vtX = X_AX ; vtX.LocToLoc( m_MapFrame, frB) ;
|
||||
Vector3d vtY = Y_AX ; vtY.LocToLoc( m_MapFrame, frB) ;
|
||||
|
||||
// Ciclo di intersezione dei dexel con il BBox
|
||||
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 ;
|
||||
for ( int k = 0 ; k < 5 ; ++ k) {
|
||||
Point3d ptT = ptO + ( i + 0.5) * m_dStep * vtX + ( j + 0.5) * m_dStep * vtY ;
|
||||
if ( k == 0)
|
||||
;
|
||||
else if ( k == 1)
|
||||
ptT += - 0.4 * m_dStep * vtX - 0.4 * m_dStep * vtY ;
|
||||
else if ( k == 2)
|
||||
ptT += + 0.4 * m_dStep * vtX - 0.4 * m_dStep * vtY ;
|
||||
else if ( k == 3)
|
||||
ptT += + 0.4 * m_dStep * vtX + 0.4 * m_dStep * vtY ;
|
||||
else if ( k == 4)
|
||||
ptT += - 0.4 * m_dStep * vtX + 0.4 * m_dStep * vtY ;
|
||||
double dZmin, dZmax ;
|
||||
if ( IntersLineBox( ptT, vtK, ORIG, ORIG + vtDg, dZmin, dZmax)) {
|
||||
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
|
||||
if ( ! ( dZmax < m_Values[0][nPos][nIndex].dMin - EPS_SMALL ||
|
||||
dZmin > m_Values[0][nPos][nIndex].dMax + EPS_SMALL))
|
||||
return false ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// altrimenti verifico con tutte e tre le mappe
|
||||
else {
|
||||
return false ;
|
||||
}
|
||||
|
||||
return true ;
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
bool
|
||||
VolZmap::AvoidSphere( const Point3d& ptCenter, double dRad, double dSafeDist) const
|
||||
VolZmap::AvoidSphere( const Point3d& ptCenter, double dRad, double dSafeDist, bool bPrecise) const
|
||||
{
|
||||
// Porto la sfera nel riferimento intrinseco dello Zmap
|
||||
Point3d ptC = ptCenter ;
|
||||
@@ -597,51 +606,60 @@ VolZmap::AvoidSphere( const Point3d& ptCenter, double dRad, double dSafeDist) co
|
||||
if ( ! b3Zmap.FindIntersection( b3Box, b3Int))
|
||||
return true ;
|
||||
|
||||
// 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) ;
|
||||
int nStJ = Clamp( int( b3Int.GetMin().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
int nEnJ = Clamp( int( b3Int.GetMax().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
// Se verifico solo prima mappa
|
||||
if ( ! bPrecise) {
|
||||
|
||||
// Ciclo di intersezione dei dexel con la sfera (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 ;
|
||||
for ( int k = 0 ; k < 5 ; ++ k) {
|
||||
Point3d ptT = ORIG + ( i + 0.5) * m_dStep * X_AX + ( j + 0.5) * m_dStep * Y_AX ;
|
||||
if ( k == 0)
|
||||
;
|
||||
else if ( k == 1)
|
||||
ptT += - 0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 2)
|
||||
ptT += + 0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 3)
|
||||
ptT += + 0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 4)
|
||||
ptT += - 0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
Point3d ptI1, ptI2 ;
|
||||
if ( ::IntersLineSphere( ptT, Z_AX, ptC, dRad, ptI1, ptI2) != ILST_NO) {
|
||||
double dZmin = min( ptI1.z, ptI2.z) ;
|
||||
double dZmax = max( ptI1.z, ptI2.z) ;
|
||||
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
|
||||
if ( ! ( dZmax < m_Values[0][nPos][nIndex].dMin - EPS_SMALL ||
|
||||
dZmin > m_Values[0][nPos][nIndex].dMax + EPS_SMALL))
|
||||
return false ;
|
||||
// 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) ;
|
||||
int nStJ = Clamp( int( b3Int.GetMin().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
int nEnJ = Clamp( int( b3Int.GetMax().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
|
||||
// Ciclo di intersezione dei dexel con la sfera (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 ;
|
||||
for ( int k = 0 ; k < 5 ; ++ k) {
|
||||
Point3d ptT = ORIG + ( i + 0.5) * m_dStep * X_AX + ( j + 0.5) * m_dStep * Y_AX ;
|
||||
if ( k == 0)
|
||||
;
|
||||
else if ( k == 1)
|
||||
ptT += - 0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 2)
|
||||
ptT += + 0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 3)
|
||||
ptT += + 0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 4)
|
||||
ptT += - 0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
Point3d ptI1, ptI2 ;
|
||||
if ( ::IntersLineSphere( ptT, Z_AX, ptC, dRad, ptI1, ptI2) != ILST_NO) {
|
||||
double dZmin = min( ptI1.z, ptI2.z) ;
|
||||
double dZmax = max( ptI1.z, ptI2.z) ;
|
||||
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
|
||||
if ( ! ( dZmax < m_Values[0][nPos][nIndex].dMin - EPS_SMALL ||
|
||||
dZmin > m_Values[0][nPos][nIndex].dMax + EPS_SMALL))
|
||||
return false ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// altrimenti verifico con tutte e tre le mappe
|
||||
else {
|
||||
return false ;
|
||||
}
|
||||
|
||||
return true ;
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
bool
|
||||
VolZmap::AvoidCylinder( const Frame3d& frCyl, double dH, double dR, double dSafeDist) const
|
||||
VolZmap::AvoidCylinder( const Frame3d& frCyl, double dH, double dR, double dSafeDist, bool bPrecise) const
|
||||
{
|
||||
// Porto il cilindro nel riferimento intrinseco dello Zmap
|
||||
Frame3d frC = frCyl ;
|
||||
@@ -685,46 +703,261 @@ VolZmap::AvoidCylinder( const Frame3d& frCyl, double dH, double dR, double dSafe
|
||||
if ( ! b3Zmap.FindIntersection( b3Box, b3Int))
|
||||
return true ;
|
||||
|
||||
// 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) ;
|
||||
int nStJ = Clamp( int( b3Int.GetMin().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
int nEnJ = Clamp( int( b3Int.GetMax().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
// Se verifico solo prima mappa
|
||||
if ( ! bPrecise) {
|
||||
|
||||
// Ciclo di intersezione dei dexel con il cilindro (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 ;
|
||||
for ( int k = 0 ; k < 5 ; ++ k) {
|
||||
Point3d ptT = ORIG + ( i + 0.5) * m_dStep * X_AX + ( j + 0.5) * m_dStep * Y_AX ;
|
||||
if ( k == 0)
|
||||
;
|
||||
else if ( k == 1)
|
||||
ptT += - 0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 2)
|
||||
ptT += + 0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 3)
|
||||
ptT += + 0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 4)
|
||||
ptT += - 0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
Point3d ptI1, ptI2 ;
|
||||
Vector3d vtN1, vtN2 ;
|
||||
if ( IntersLineCylinder( ptT, Z_AX, frC, dH, dR, true, true, ptI1, vtN1, ptI2, vtN2)) {
|
||||
double dZmin = min( ptI1.z, ptI2.z) ;
|
||||
double dZmax = max( ptI1.z, ptI2.z) ;
|
||||
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
|
||||
if ( ! ( dZmax < m_Values[0][nPos][nIndex].dMin - EPS_SMALL ||
|
||||
dZmin > m_Values[0][nPos][nIndex].dMax + EPS_SMALL))
|
||||
return false ;
|
||||
// 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) ;
|
||||
int nStJ = Clamp( int( b3Int.GetMin().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
int nEnJ = Clamp( int( b3Int.GetMax().y / m_dStep), 0, m_nNy[0] - 1) ;
|
||||
|
||||
// Ciclo di intersezione dei dexel con il cilindro (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 ;
|
||||
for ( int k = 0 ; k < 5 ; ++ k) {
|
||||
Point3d ptT = ORIG + ( i + 0.5) * m_dStep * X_AX + ( j + 0.5) * m_dStep * Y_AX ;
|
||||
if ( k == 0)
|
||||
;
|
||||
else if ( k == 1)
|
||||
ptT += - 0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 2)
|
||||
ptT += + 0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 3)
|
||||
ptT += + 0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 4)
|
||||
ptT += - 0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
Point3d ptI1, ptI2 ;
|
||||
Vector3d vtN1, vtN2 ;
|
||||
if ( IntersLineCylinder( ptT, Z_AX, frC, dH, dR, true, true, ptI1, vtN1, ptI2, vtN2)) {
|
||||
double dZmin = min( ptI1.z, ptI2.z) ;
|
||||
double dZmax = max( ptI1.z, ptI2.z) ;
|
||||
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
|
||||
if ( ! ( dZmax < m_Values[0][nPos][nIndex].dMin - EPS_SMALL ||
|
||||
dZmin > m_Values[0][nPos][nIndex].dMax + EPS_SMALL))
|
||||
return false ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// altrimenti verifico con tutte e tre le mappe
|
||||
else {
|
||||
return false ;
|
||||
}
|
||||
|
||||
return true ;
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
bool
|
||||
VolZmap::AvoidTruncatedCone( const Point3d& ptBase, const Vector3d vtSymmetryAx,
|
||||
double dRadTop, double dRadBot, double dHeight, bool bPrecise) const
|
||||
{
|
||||
// Porto il tronco di cono nel sistema intrinseco e normalizzo il vettore.
|
||||
Point3d ptMyBase = ptBase ;
|
||||
Vector3d vtMySymmetryAx = vtSymmetryAx ;
|
||||
ptMyBase.ToLoc( m_MapFrame) ;
|
||||
vtMySymmetryAx.ToLoc( m_MapFrame) ;
|
||||
vtMySymmetryAx.Normalize() ;
|
||||
|
||||
// Se i BoundingBox non si sovrappongono, ho finito
|
||||
BBox3d b3ConusBox( ptMyBase + 0.5 * dHeight * vtMySymmetryAx) ;
|
||||
b3ConusBox.Expand( dHeight + max( dRadTop, dRadBot)) ;
|
||||
BBox3d b3Zmap( ORIG, Point3d( m_nNx[0] * m_dStep, m_nNy[0] * m_dStep, m_dMaxZ[0])) ;
|
||||
if ( ! b3Zmap.FindIntersection( b3ConusBox, BBox3d()))
|
||||
return true ;
|
||||
|
||||
// Definisco un sistema di riferimento del tronco cono con origine nel vertice virtuale
|
||||
// e asse Z coincidente con l'asse di simmetria e diretto verso l'apertura.
|
||||
double dMinRad, dMaxRad ;
|
||||
Point3d ptMinBase ;
|
||||
Vector3d vtAx ;
|
||||
if ( dRadTop < dRadBot) {
|
||||
dMinRad = dRadTop ;
|
||||
dMaxRad = dRadBot ;
|
||||
vtAx = - vtMySymmetryAx ;
|
||||
ptMinBase = ptMyBase - dHeight * vtAx ;
|
||||
}
|
||||
else {
|
||||
dMinRad = dRadBot ;
|
||||
dMaxRad = dRadTop ;
|
||||
vtAx = vtMySymmetryAx ;
|
||||
ptMinBase = ptMyBase ;
|
||||
}
|
||||
double dTotHeight = dHeight * dMaxRad / ( dMaxRad - dMinRad) ;
|
||||
double dDeltaHeight = dTotHeight - dHeight ;
|
||||
Point3d ptVert = ptMinBase - dDeltaHeight * vtAx ;
|
||||
Frame3d frConusFrame ;
|
||||
frConusFrame.Set( ptVert, vtAx) ;
|
||||
|
||||
// Ciclo sulle mappe.
|
||||
int nMapNum = bPrecise ? min( m_nMapNum, 3) : 1 ;
|
||||
for ( int nMap = 0 ; nMap < nMapNum ; ++ nMap) {
|
||||
for ( int nDex = 0 ; nDex < int( m_Values[nMap].size()) ; ++ nDex) {
|
||||
// Indici del dexel.
|
||||
int nI = nDex % m_nNx[nMap] ;
|
||||
int nJ = nDex / m_nNx[nMap] ;
|
||||
// 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).
|
||||
if ( nMap == 1) {
|
||||
swap( ptLineSt.x, ptLineSt.y) ;
|
||||
swap( ptLineSt.x, ptLineSt.z) ;
|
||||
swap( vtLineDir.x, vtLineDir.y) ;
|
||||
swap( vtLineDir.x, vtLineDir.z) ;
|
||||
}
|
||||
else if ( nMap == 2) {
|
||||
swap( ptLineSt.x, ptLineSt.y) ;
|
||||
swap( ptLineSt.y, ptLineSt.z) ;
|
||||
swap( vtLineDir.x, vtLineDir.y) ;
|
||||
swap( vtLineDir.y, vtLineDir.z) ;
|
||||
}
|
||||
double dTan = dMaxRad / dTotHeight ;
|
||||
Point3d ptInt1, ptInt2 ;
|
||||
// La retta del dexel interseca il tronco di cono.
|
||||
if ( IntersLineConus( ptLineSt, vtLineDir, frConusFrame, dTan, dDeltaHeight, dTotHeight, true, true,
|
||||
ptInt1, Vector3d( 0., 0., 0.), ptInt2, Vector3d( 0., 0., 0.))) {
|
||||
double dMinU, dMaxU ;
|
||||
if ( nMap == 0) {
|
||||
dMinU = min( ptInt1.z, ptInt2.z) ;
|
||||
dMaxU = max( ptInt1.z, ptInt2.z) ;
|
||||
}
|
||||
else if ( nMap == 1) {
|
||||
dMinU = min( ptInt1.x, ptInt2.x) ;
|
||||
dMaxU = max( ptInt1.x, ptInt2.x) ;
|
||||
}
|
||||
else {
|
||||
dMinU = min( ptInt1.y, ptInt2.y) ;
|
||||
dMaxU = max( ptInt1.y, ptInt2.y) ;
|
||||
}
|
||||
// Ciclo sui segmenti del dexel.
|
||||
for ( int nInt = 0 ; nInt < int( m_Values[nMap][nDex].size()) ; ++ nInt) {
|
||||
double dMin = m_Values[nMap][nDex][nInt].dMin ;
|
||||
double dMax = m_Values[nMap][nDex][nInt].dMax ;
|
||||
// Se c'è intersezione, ho finito.
|
||||
if ( ! ( dMax < dMinU - EPS_SMALL || dMin > dMaxU + EPS_SMALL))
|
||||
return false ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return true ;
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
bool
|
||||
VolZmap::AvoidTruncatedPyramid( const Point3d& ptBase, const Vector3d& vtAxZ, const Vector3d& vtAxX,
|
||||
double dSegTop, double dSegBot, double dHeight, bool bPrecise) const
|
||||
{
|
||||
// Porto il tronco di piramide nel sistema intrinseco e normalizzo i vettori.
|
||||
Point3d ptMyBase = ptBase ;
|
||||
Vector3d vtMyAxZ = vtAxZ ;
|
||||
Vector3d vtMyAxX = vtAxX ;
|
||||
ptMyBase.ToLoc( m_MapFrame) ;
|
||||
vtMyAxZ.ToLoc( m_MapFrame) ;
|
||||
vtMyAxX.ToLoc( m_MapFrame) ;
|
||||
vtMyAxZ.Normalize() ;
|
||||
vtMyAxX.Normalize() ;
|
||||
|
||||
// Se i BoundingBox non si sovrappongono, ho finito
|
||||
Vector3d vtMyAxY = vtMyAxZ ^ vtMyAxX ;
|
||||
vtMyAxY.Normalize() ;
|
||||
BBox3d b3PyramyBox ;
|
||||
b3PyramyBox.Add( ptMyBase - 0.5 * dSegBot * ( vtMyAxX + vtMyAxY)) ;
|
||||
b3PyramyBox.Add( ptMyBase + 0.5 * dSegBot * ( vtMyAxX - vtMyAxY)) ;
|
||||
b3PyramyBox.Add( ptMyBase + 0.5 * dSegBot * ( vtMyAxX + vtMyAxY)) ;
|
||||
b3PyramyBox.Add( ptMyBase - 0.5 * dSegBot * ( vtMyAxX - vtMyAxY)) ;
|
||||
b3PyramyBox.Add( ptMyBase - 0.5 * dSegBot * ( vtMyAxX + vtMyAxY) + dHeight * vtMyAxZ) ;
|
||||
b3PyramyBox.Add( ptMyBase + 0.5 * dSegBot * ( vtMyAxX - vtMyAxY) + dHeight * vtMyAxZ) ;
|
||||
b3PyramyBox.Add( ptMyBase + 0.5 * dSegBot * ( vtMyAxX + vtMyAxY) + dHeight * vtMyAxZ) ;
|
||||
b3PyramyBox.Add( ptMyBase - 0.5 * dSegBot * ( vtMyAxX - vtMyAxY) + dHeight * vtMyAxZ) ;
|
||||
BBox3d b3Zmap( ORIG, Point3d( m_nNx[0] * m_dStep, m_nNy[0] * m_dStep, m_dMaxZ[0])) ;
|
||||
if ( ! b3Zmap.FindIntersection( b3PyramyBox, BBox3d()))
|
||||
return true ;
|
||||
|
||||
// Definisco un sistema di riferimento con origine nel centro della base maggiore del tronco di piramide,
|
||||
// asse Z ortogonale alle basi diretto verso la minore e asse X lungo un segmento della base maggiore.
|
||||
double dMinSeg, dMaxSeg ;
|
||||
Point3d ptMaxBase ;
|
||||
Vector3d vtAx ;
|
||||
if ( dSegTop < dSegBot) {
|
||||
dMinSeg = dSegTop ;
|
||||
dMaxSeg = dSegBot ;
|
||||
vtAx = vtMyAxZ ;
|
||||
ptMaxBase = ptMyBase ;
|
||||
}
|
||||
else {
|
||||
dMinSeg = dSegBot ;
|
||||
dMaxSeg = dSegTop ;
|
||||
vtAx = - vtMyAxZ ;
|
||||
ptMaxBase = ptMyBase - dHeight * vtAx ;
|
||||
}
|
||||
Frame3d frPyramidFrame ;
|
||||
frPyramidFrame.Set( ptMaxBase, vtMyAxX, vtAx ^ vtMyAxX, vtAx) ;
|
||||
|
||||
// Ciclo sulle mappe
|
||||
int nMapNum = bPrecise ? min( m_nMapNum, 3) : 1 ;
|
||||
for ( int nMap = 0 ; nMap < nMapNum ; ++ nMap) {
|
||||
for ( int nDex = 0 ; nDex < int( m_Values[nMap].size()) ; ++ nDex) {
|
||||
// Indici del dexel
|
||||
int nI = nDex % m_nNx[nMap] ;
|
||||
int nJ = nDex / m_nNx[nMap] ;
|
||||
// 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).
|
||||
if ( nMap == 1) {
|
||||
swap( ptLineSt.x, ptLineSt.y) ;
|
||||
swap( ptLineSt.x, ptLineSt.z) ;
|
||||
swap( vtLineDir.x, vtLineDir.y) ;
|
||||
swap( vtLineDir.x, vtLineDir.z) ;
|
||||
}
|
||||
else if ( nMap == 2) {
|
||||
swap( ptLineSt.x, ptLineSt.y) ;
|
||||
swap( ptLineSt.y, ptLineSt.z) ;
|
||||
swap( vtLineDir.x, vtLineDir.y) ;
|
||||
swap( vtLineDir.y, vtLineDir.z) ;
|
||||
}
|
||||
Point3d ptInt1, ptInt2 ;
|
||||
// La retta del dexel interseca il tronco di piramide.
|
||||
if ( IntersLineTruncatedPyramid( ptLineSt, vtLineDir, frPyramidFrame, dMinSeg, dMaxSeg, dHeight,
|
||||
ptInt1, Vector3d( 0., 0., 0.), ptInt2, Vector3d( 0., 0., 0.))) {
|
||||
double dMinU, dMaxU ;
|
||||
if ( nMap == 0) {
|
||||
dMinU = min( ptInt1.z, ptInt2.z) ;
|
||||
dMaxU = max( ptInt1.z, ptInt2.z) ;
|
||||
}
|
||||
else if ( nMap == 1) {
|
||||
dMinU = min( ptInt1.x, ptInt2.x) ;
|
||||
dMaxU = max( ptInt1.x, ptInt2.x) ;
|
||||
}
|
||||
else {
|
||||
dMinU = min( ptInt1.y, ptInt2.y) ;
|
||||
dMaxU = max( ptInt1.y, ptInt2.y) ;
|
||||
}
|
||||
// Ciclo sui segmenti del dexel.
|
||||
for ( int nInt = 0 ; nInt < int( m_Values[nMap][nDex].size()) ; ++ nInt) {
|
||||
double dMin = m_Values[nMap][nDex][nInt].dMin ;
|
||||
double dMax = m_Values[nMap][nDex][nInt].dMax ;
|
||||
// Se c'è intersezione, ho finito.
|
||||
if ( ! ( dMax < dMinU - EPS_SMALL || dMin > dMaxU + EPS_SMALL))
|
||||
return false ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return true ;
|
||||
}
|
||||
|
||||
@@ -1117,6 +1350,11 @@ VolZmap::IntersLineEllipticalCylinder( const Point3d& ptLineSt, const Vector3d&
|
||||
else
|
||||
return false ;
|
||||
|
||||
// Se il secondo punto è sceso in Z sotto il primo o il primo è salito sopra il secondo vuol
|
||||
// dire che l'intersezione è al limite del cilindro ellittico; non c'è reale intersezione.
|
||||
if ( ptInt1.z > ptInt2.z)
|
||||
return false ;
|
||||
|
||||
// Porto i punti e i versori nel riferimento globale
|
||||
ptInt1.ToGlob( CircFrame) ;
|
||||
vtN1.ToGlob( CircFrame) ;
|
||||
@@ -1275,6 +1513,174 @@ VolZmap::IntersLineMyPolyhedron( const Point3d& ptLineSt, const Vector3d& vtLine
|
||||
return true ;
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
bool
|
||||
VolZmap::IntersLineTruncatedPyramid( const Point3d& ptLineSt, const Vector3d& vtLineDir,
|
||||
const Frame3d& frTruncPyramFrame, double dSegMin, double dSegMax, double dHeight,
|
||||
Point3d& ptInt1, Vector3d& vtN1, Point3d& ptInt2, Vector3d& vtN2) const
|
||||
{
|
||||
// Controllo sulle dimensioni lineari affinché sia valido il solido
|
||||
if ( dSegMin < EPS_SMALL || dSegMax < EPS_SMALL || dHeight < EPS_SMALL)
|
||||
return false ;
|
||||
|
||||
// Porto la linea nel riferimento del solido
|
||||
Point3d ptP = ptLineSt ;
|
||||
Vector3d vtV = vtLineDir ;
|
||||
ptP.ToLoc( frTruncPyramFrame) ;
|
||||
vtV.ToLoc( frTruncPyramFrame) ;
|
||||
|
||||
// Se la retta sta sopra o sotto il solido non vi può essere intersezione
|
||||
if ( abs( vtV.z) < EPS_ZERO && ( ptP.z < EPS_SMALL || ptP.z > dHeight + EPS_SMALL))
|
||||
return false ;
|
||||
|
||||
// Cerco le intersezioni con i piani delle facce
|
||||
int nIntNum = 0 ;
|
||||
// Base maggiore
|
||||
Point3d ptIntPlaneMax = ptP + ( - ptP.z / vtV.z) * vtV ;
|
||||
if ( abs( ptIntPlaneMax.x) <= dSegMax && abs( ptIntPlaneMax.y) <= dSegMax) {
|
||||
ptInt1 = ptIntPlaneMax ;
|
||||
vtN1 = - Z_AX ;
|
||||
++ nIntNum ;
|
||||
}
|
||||
// Base minore
|
||||
Point3d ptIntPlaneMin = ptP + ( ( dHeight - ptP.z) / vtV.z) * vtV ;
|
||||
if ( abs( ptIntPlaneMin.x) <= dSegMin && abs( ptIntPlaneMin.y) <= dSegMin) {
|
||||
if ( nIntNum == 0) {
|
||||
ptInt1 = ptIntPlaneMin ;
|
||||
vtN1 = Z_AX ;
|
||||
}
|
||||
else {
|
||||
ptInt2 = ptIntPlaneMin ;
|
||||
vtN2 = Z_AX ;
|
||||
}
|
||||
++ nIntNum ;
|
||||
}
|
||||
|
||||
// Se la retta intersca i piani delle basi al loro interno, ho finito
|
||||
if ( nIntNum == 2) {
|
||||
if ( ( ptInt1 - ptP) * vtV > ( ptInt2 - ptP) * vtV) {
|
||||
swap( ptInt1, ptInt2) ;
|
||||
swap( vtN1, vtN2) ;
|
||||
}
|
||||
ptInt1.ToGlob( frTruncPyramFrame) ;
|
||||
ptInt2.ToGlob( frTruncPyramFrame) ;
|
||||
vtN1.ToGlob( frTruncPyramFrame) ;
|
||||
vtN2.ToGlob( frTruncPyramFrame) ;
|
||||
return true ;
|
||||
}
|
||||
|
||||
// Altrimenti se le intersezioni sono fuori dalle basi e dallo stesso lato rispetto a queste ultime non c'è intersezione
|
||||
if ( nIntNum == 0 &&
|
||||
( ( ptIntPlaneMax.x < - dSegMax - EPS_SMALL && ptIntPlaneMin.x < - dSegMin - EPS_SMALL) ||
|
||||
( ptIntPlaneMax.x > dSegMax + EPS_SMALL && ptIntPlaneMin.x > dSegMin + EPS_SMALL) ||
|
||||
( ptIntPlaneMax.y < - dSegMax - EPS_SMALL && ptIntPlaneMin.y < - dSegMin - EPS_SMALL) ||
|
||||
( ptIntPlaneMax.y > dSegMax + EPS_SMALL && ptIntPlaneMin.y > dSegMin + EPS_SMALL)))
|
||||
return false ;
|
||||
|
||||
// Cerco le eventuali intersezioni con i piani delle facce laterali
|
||||
Point3d ptPlaneP( - dSegMax, - dSegMax, 0.) ;
|
||||
Vector3d vtPlaneN( 0., - dHeight, ( dSegMax - dSegMin)) ;
|
||||
vtPlaneN.Normalize() ;
|
||||
if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) {
|
||||
if ( nIntNum == 0) {
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
vtN1 = vtPlaneN ;
|
||||
if ( ptInt1.z >= 0 && ptInt1.z <= dHeight &&
|
||||
( ptInt1.x + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt1.z &&
|
||||
( ptInt1.x - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt1.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
else {
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
vtN2 = vtPlaneN ;
|
||||
if ( ( ptInt2 - ptInt2).SqLen() > SQ_EPS_SMALL &&
|
||||
ptInt2.z >= 0 && ptInt1.z <= dHeight &&
|
||||
( ptInt2.x + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt2.z &&
|
||||
( ptInt2.x - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt2.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
}
|
||||
vtPlaneN = Vector3d( - dHeight, 0., ( dSegMax - dSegMin)) ;
|
||||
vtPlaneN.Normalize() ;
|
||||
if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) {
|
||||
if ( nIntNum == 0) {
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
vtN1 = vtPlaneN ;
|
||||
if ( ptInt1.z >= 0 && ptInt1.z <= dHeight &&
|
||||
( ptInt1.y + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt1.z &&
|
||||
( ptInt1.y - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt1.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
else {
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
vtN2 = vtPlaneN ;
|
||||
if ( ( ptInt1 - ptInt2).SqLen() > SQ_EPS_SMALL &&
|
||||
ptInt2.z >= 0 && ptInt2.z <= dHeight &&
|
||||
( ptInt2.y + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt2.z &&
|
||||
( ptInt2.y - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt2.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
}
|
||||
ptPlaneP = Point3d( dSegMax, dSegMax, 0.) ;
|
||||
vtPlaneN = Vector3d( dHeight, 0., ( dSegMax - dSegMin)) ;
|
||||
vtPlaneN.Normalize() ;
|
||||
if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) {
|
||||
if ( nIntNum == 0) {
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
vtN1 = vtPlaneN ;
|
||||
if ( ptInt1.z >= 0 && ptInt1.z <= dHeight &&
|
||||
( ptInt1.y + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt1.z &&
|
||||
( ptInt1.y - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt1.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
else {
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
vtN2 = vtPlaneN ;
|
||||
if ( ( ptInt1 - ptInt2).SqLen() > SQ_EPS_SMALL &&
|
||||
ptInt2.z >= 0 && ptInt2.z <= dHeight &&
|
||||
( ptInt2.y + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt2.z &&
|
||||
( ptInt2.y - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt2.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
}
|
||||
vtPlaneN = Vector3d( 0., dHeight, ( dSegMax - dSegMin)) ;
|
||||
vtPlaneN.Normalize() ;
|
||||
if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) {
|
||||
if ( nIntNum == 0) {
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
vtN1 = vtPlaneN ;
|
||||
if ( ptInt1.z >= 0 && ptInt1.z <= dHeight &&
|
||||
( ptInt1.x + dSegMax) * dHeight >= (dSegMax - dSegMin) * ptInt1.z &&
|
||||
( ptInt1.x - dSegMax) * dHeight <= (dSegMin - dSegMax) * ptInt1.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
else {
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
vtN2 = vtPlaneN ;
|
||||
if ( ( ptInt1 - ptInt2).SqLen() > SQ_EPS_SMALL &&
|
||||
ptInt2.z >= 0 && ptInt1.z <= dHeight &&
|
||||
( ptInt2.x + dSegMax) * dHeight >= ( dSegMax - dSegMin) * ptInt2.z &&
|
||||
( ptInt2.x - dSegMax) * dHeight <= ( dSegMin - dSegMax) * ptInt2.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
}
|
||||
|
||||
// La retta interseca il tronco di piramide
|
||||
if ( nIntNum == 2) {
|
||||
if ( ( ptInt1 - ptP) * vtV > ( ptInt2 - ptP) * vtV) {
|
||||
swap( ptInt1, ptInt2) ;
|
||||
swap( vtN1, vtN2) ;
|
||||
}
|
||||
ptInt1.ToGlob( frTruncPyramFrame) ;
|
||||
ptInt2.ToGlob( frTruncPyramFrame) ;
|
||||
vtN1.ToGlob( frTruncPyramFrame) ;
|
||||
vtN2.ToGlob( frTruncPyramFrame) ;
|
||||
return true ;
|
||||
}
|
||||
|
||||
return false ;
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
bool
|
||||
VolZmap::GetVolume( double& dVol) const
|
||||
|
||||
Reference in New Issue
Block a user