EgtGeomKernel 2.2k1 :
- aggiunte funzioni per verifica di collisione tra Tronco di Cono e Triangolo e tra Tronco di Piramide Generalizzato e Triangolo - migliorate funzioni per verifica di collisione tra Tronco di Cono e Zmap e tra Tronco di Piramide Generalizzato e Zmap.
This commit is contained in:
Dario Sassi
+464
-131
@@ -576,7 +576,61 @@ VolZmap::AvoidBox( const Frame3d& frBox, const Vector3d& vtDiag, double dSafeDis
|
||||
|
||||
// altrimenti verifico con tutte e tre le mappe
|
||||
else {
|
||||
return false ;
|
||||
// Ciclo di intersezione dei dexel con il BBox
|
||||
for ( int nMap = 0 ; nMap < m_nMapNum ; ++ nMap) {
|
||||
Point3d ptO = ORIG ;
|
||||
Vector3d vtX = X_AX ;
|
||||
Vector3d vtY = Y_AX ;
|
||||
Vector3d vtK = Z_AX ;
|
||||
// Estremi del box
|
||||
Point3d ptBoxInf = b3Int.GetMin() ;
|
||||
Point3d ptBoxSup = b3Int.GetMax() ;
|
||||
if ( nMap == 1) {
|
||||
swap( ptBoxInf.x, ptBoxInf.z) ;
|
||||
swap( ptBoxInf.x, ptBoxInf.y) ;
|
||||
swap( ptBoxSup.x, ptBoxSup.z) ;
|
||||
swap( ptBoxSup.x, ptBoxSup.y) ;
|
||||
vtX = Y_AX ;
|
||||
vtY = Z_AX ;
|
||||
vtK = X_AX ;
|
||||
}
|
||||
else if ( nMap == 2) {
|
||||
swap( ptBoxInf.y, ptBoxInf.z) ;
|
||||
swap( ptBoxInf.x, ptBoxInf.y) ;
|
||||
swap( ptBoxSup.y, ptBoxSup.z) ;
|
||||
swap( ptBoxSup.x, ptBoxSup.y) ;
|
||||
vtX = Z_AX ;
|
||||
vtY = X_AX ;
|
||||
vtK = Y_AX ;
|
||||
}
|
||||
// Passo da sistema griglia a sistema BBox
|
||||
ptO.ToLoc( frB) ;
|
||||
vtX.ToLoc( frB) ;
|
||||
vtY.ToLoc( frB) ;
|
||||
vtK.ToLoc( frB) ;
|
||||
// Limiti su indici
|
||||
int nStI = Clamp( int( ptBoxInf.x / m_dStep), 0, m_nNx[nMap] - 1) ;
|
||||
int nEnI = Clamp( int( ptBoxSup.x / m_dStep), 0, m_nNx[nMap] - 1) ;
|
||||
int nStJ = Clamp( int( ptBoxInf.y / m_dStep), 0, m_nNy[nMap] - 1) ;
|
||||
int nEnJ = Clamp( int( ptBoxSup.y / m_dStep), 0, m_nNy[nMap] - 1) ;
|
||||
for ( int i = nStI ; i <= nEnI ; ++ i) {
|
||||
for ( int j = nStJ ; j <= nEnJ ; ++ j) {
|
||||
int nPos = j * m_nNx[nMap] + i;
|
||||
int nSize = int( m_Values[nMap][nPos].size()) ;
|
||||
if ( nSize == 0)
|
||||
continue ;
|
||||
Point3d ptT = ptO + ( i + 0.5) * m_dStep * vtX + ( j + 0.5) * m_dStep * vtY ;
|
||||
double dMinU, dMaxU ;
|
||||
if ( IntersLineBox( ptT, vtK, ORIG, ORIG + vtDg, dMinU, dMaxU)) {
|
||||
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
|
||||
if ( ! ( dMaxU < m_Values[nMap][nPos][nIndex].dMin - EPS_SMALL ||
|
||||
dMinU > m_Values[nMap][nPos][nIndex].dMax + EPS_SMALL))
|
||||
return false ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true ;
|
||||
@@ -651,7 +705,68 @@ VolZmap::AvoidSphere( const Point3d& ptCenter, double dRad, double dSafeDist, bo
|
||||
|
||||
// altrimenti verifico con tutte e tre le mappe
|
||||
else {
|
||||
return false ;
|
||||
// Ciclo di intersezione dei dexel con la sfera (nel riferimento intrinseco)
|
||||
for ( int nMap = 0 ; nMap < m_nMapNum ; ++ nMap) {
|
||||
Vector3d vtX = X_AX ;
|
||||
Vector3d vtY = Y_AX ;
|
||||
Vector3d vtK = Z_AX ;
|
||||
// Estremi del box
|
||||
Point3d ptBoxInf = b3Int.GetMin() ;
|
||||
Point3d ptBoxSup = b3Int.GetMax() ;
|
||||
if ( nMap == 1) {
|
||||
swap( ptBoxInf.x, ptBoxInf.z) ;
|
||||
swap( ptBoxInf.x, ptBoxInf.y) ;
|
||||
swap( ptBoxSup.x, ptBoxSup.z) ;
|
||||
swap( ptBoxSup.x, ptBoxSup.y) ;
|
||||
vtX = Y_AX ;
|
||||
vtY = Z_AX ;
|
||||
vtK = X_AX ;
|
||||
}
|
||||
else if ( nMap == 2) {
|
||||
swap( ptBoxInf.y, ptBoxInf.z) ;
|
||||
swap( ptBoxInf.x, ptBoxInf.y) ;
|
||||
swap( ptBoxSup.y, ptBoxSup.z) ;
|
||||
swap( ptBoxSup.x, ptBoxSup.y) ;
|
||||
vtX = Z_AX ;
|
||||
vtY = X_AX ;
|
||||
vtK = Y_AX ;
|
||||
}
|
||||
// Limiti su indici
|
||||
int nStI = Clamp( int( ptBoxInf.x / m_dStep), 0, m_nNx[nMap] - 1) ;
|
||||
int nEnI = Clamp( int( ptBoxSup.x / m_dStep), 0, m_nNx[nMap] - 1) ;
|
||||
int nStJ = Clamp( int( ptBoxInf.y / m_dStep), 0, m_nNy[nMap] - 1) ;
|
||||
int nEnJ = Clamp( int( ptBoxSup.y / m_dStep), 0, m_nNy[nMap] - 1) ;
|
||||
for ( int i = nStI ; i <= nEnI ; ++ i) {
|
||||
for ( int j = nStJ ; j <= nEnJ ; ++ j) {
|
||||
int nPos = j * m_nNx[nMap] + i ;
|
||||
int nSize = int( m_Values[nMap][nPos].size()) ;
|
||||
if ( nSize == 0)
|
||||
continue ;
|
||||
Point3d ptT = ORIG + ( i + 0.5) * m_dStep * vtX + ( j + 0.5) * m_dStep * vtY ;
|
||||
Point3d ptI1, ptI2 ;
|
||||
if ( ::IntersLineSphere( ptT, vtK, ptC, dRad, ptI1, ptI2) != ILST_NO) {
|
||||
double dMinU, dMaxU ;
|
||||
if ( nMap == 0) {
|
||||
dMinU = min( ptI1.z, ptI2.z) ;
|
||||
dMaxU = max( ptI1.z, ptI2.z) ;
|
||||
}
|
||||
else if ( nMap == 1) {
|
||||
dMinU = min( ptI1.x, ptI2.x) ;
|
||||
dMaxU = max( ptI1.x, ptI2.x) ;
|
||||
}
|
||||
else {
|
||||
dMinU = min( ptI1.y, ptI2.y) ;
|
||||
dMaxU = max( ptI1.y, ptI2.y) ;
|
||||
}
|
||||
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
|
||||
if ( ! ( dMaxU < m_Values[nMap][nPos][nIndex].dMin - EPS_SMALL ||
|
||||
dMinU > m_Values[nMap][nPos][nIndex].dMax + EPS_SMALL))
|
||||
return false ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true ;
|
||||
@@ -749,7 +864,73 @@ VolZmap::AvoidCylinder( const Frame3d& frCyl, double dH, double dR, double dSafe
|
||||
|
||||
// altrimenti verifico con tutte e tre le mappe
|
||||
else {
|
||||
return false ;
|
||||
// Ciclo di intersezione dei dexel con il cilindro (nel riferimento intrinseco)
|
||||
for ( int nMap = 0 ; nMap < m_nMapNum ; ++ nMap) {
|
||||
Vector3d vtX = X_AX ;
|
||||
Vector3d vtY = Y_AX ;
|
||||
Vector3d vtK = Z_AX ;
|
||||
// Estremi del box
|
||||
Point3d ptBoxInf = b3Int.GetMin() ;
|
||||
Point3d ptBoxSup = b3Int.GetMax() ;
|
||||
if ( nMap == 1) {
|
||||
swap( ptBoxInf.x, ptBoxInf.z) ;
|
||||
swap( ptBoxInf.x, ptBoxInf.y) ;
|
||||
swap( ptBoxSup.x, ptBoxSup.z) ;
|
||||
swap( ptBoxSup.x, ptBoxSup.y) ;
|
||||
vtX = Y_AX ;
|
||||
vtY = Z_AX ;
|
||||
vtK = X_AX ;
|
||||
}
|
||||
else if ( nMap == 2) {
|
||||
swap( ptBoxInf.y, ptBoxInf.z) ;
|
||||
swap( ptBoxInf.x, ptBoxInf.y) ;
|
||||
swap( ptBoxSup.y, ptBoxSup.z) ;
|
||||
swap( ptBoxSup.x, ptBoxSup.y) ;
|
||||
vtX = Z_AX ;
|
||||
vtY = X_AX ;
|
||||
vtK = Y_AX ;
|
||||
}
|
||||
// Limiti su indici
|
||||
int nStI = Clamp( int( ptBoxInf.x / m_dStep), 0, m_nNx[nMap] - 1) ;
|
||||
int nEnI = Clamp( int( ptBoxSup.x / m_dStep), 0, m_nNx[nMap] - 1) ;
|
||||
int nStJ = Clamp( int( ptBoxInf.y / m_dStep), 0, m_nNy[nMap] - 1) ;
|
||||
int nEnJ = Clamp( int( ptBoxSup.y / m_dStep), 0, m_nNy[nMap] - 1) ;
|
||||
for ( int i = nStI ; i <= nEnI ; ++ i) {
|
||||
for ( int j = nStJ ; j <= nEnJ ; ++ j) {
|
||||
if ( nMap == 2 && j == 75)
|
||||
int a = 0 ;
|
||||
int nPos = j * m_nNx[nMap] + i ;
|
||||
int nSize = int( m_Values[nMap][nPos].size()) ;
|
||||
if ( nSize == 0)
|
||||
continue ;
|
||||
Point3d ptT = ORIG + ( i + 0.5) * m_dStep * vtX + ( j + 0.5) * m_dStep * vtY ;
|
||||
Point3d ptI1, ptI2 ;
|
||||
Vector3d vtN1, vtN2 ;
|
||||
if ( IntersLineCylinder( ptT, vtK, frC, dH, dR, true, true, ptI1, vtN1, ptI2, vtN2)) {
|
||||
double dMinU, dMaxU ;
|
||||
if ( nMap == 0) {
|
||||
dMinU = min( ptI1.z, ptI2.z) ;
|
||||
dMaxU = max( ptI1.z, ptI2.z) ;
|
||||
}
|
||||
else if ( nMap == 1) {
|
||||
dMinU = min( ptI1.x, ptI2.x) ;
|
||||
dMaxU = max( ptI1.x, ptI2.x) ;
|
||||
}
|
||||
else {
|
||||
dMinU = min( ptI1.y, ptI2.y) ;
|
||||
dMaxU = max( ptI1.y, ptI2.y) ;
|
||||
}
|
||||
for ( int nIndex = 0 ; nIndex < nSize ; nIndex += 1) {
|
||||
double dMin = m_Values[nMap][nPos][nIndex].dMin ;
|
||||
double dMax = m_Values[nMap][nPos][nIndex].dMax ;
|
||||
if ( ! ( dMaxU < dMin - EPS_SMALL ||
|
||||
dMinU > dMax + EPS_SMALL))
|
||||
return false ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true ;
|
||||
@@ -758,7 +939,7 @@ VolZmap::AvoidCylinder( const Frame3d& frCyl, double dH, double dR, double dSafe
|
||||
//----------------------------------------------------------------------------
|
||||
bool
|
||||
VolZmap::AvoidTruncatedCone( const Point3d& ptBase, const Vector3d vtSymmetryAx,
|
||||
double dRadTop, double dRadBot, double dHeight, bool bPrecise) const
|
||||
double dRadTop, double dRadBot, double dHeight, double dSafeDist, bool bPrecise) const
|
||||
{
|
||||
// Porto il tronco di cono nel sistema intrinseco e normalizzo il vettore.
|
||||
Point3d ptMyBase = ptBase ;
|
||||
@@ -767,11 +948,20 @@ VolZmap::AvoidTruncatedCone( const Point3d& ptBase, const Vector3d vtSymmetryAx,
|
||||
vtMySymmetryAx.ToLoc( m_MapFrame) ;
|
||||
vtMySymmetryAx.Normalize() ;
|
||||
|
||||
// Se distanza di sicurezza maggiore di epsilon espando il cono
|
||||
if ( dSafeDist > EPS_SMALL) {
|
||||
ptMyBase -= ( dSafeDist * vtMySymmetryAx) ;
|
||||
dHeight += ( 2 * dSafeDist) ;
|
||||
dRadTop += dSafeDist ;
|
||||
dRadBot += dSafeDist ;
|
||||
}
|
||||
|
||||
// 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()))
|
||||
BBox3d b3Int ;
|
||||
if ( ! b3Zmap.FindIntersection( b3ConusBox, b3Int))
|
||||
return true ;
|
||||
|
||||
// Definisco un sistema di riferimento del tronco cono con origine nel vertice virtuale
|
||||
@@ -797,56 +987,123 @@ VolZmap::AvoidTruncatedCone( const Point3d& ptBase, const Vector3d vtSymmetryAx,
|
||||
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).
|
||||
// Uso solo la prima mappa
|
||||
if ( ! bPrecise) {
|
||||
// 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 ptLineSt = ORIG + ( i + 0.5) * m_dStep * X_AX + ( j + 0.5) * m_dStep * Y_AX ;
|
||||
if ( k == 0)
|
||||
;
|
||||
else if ( k == 1)
|
||||
ptLineSt += -0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 2)
|
||||
ptLineSt += +0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 3)
|
||||
ptLineSt += +0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 4)
|
||||
ptLineSt += -0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
double dTan = dMaxRad / dTotHeight ;
|
||||
Point3d ptInt1, ptInt2 ;
|
||||
if ( IntersLineConus( ptLineSt, Z_AX, frConusFrame, dTan, dDeltaHeight, dTotHeight, true, true,
|
||||
ptInt1, Vector3d( 0., 0., 0.), ptInt2, Vector3d( 0., 0., 0.))) {
|
||||
double dZmin = min( ptInt1.z, ptInt2.z) ;
|
||||
double dZmax = max( ptInt1.z, ptInt2.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 ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// Uso tutte le mappe
|
||||
else {
|
||||
// Ciclo sulle mappe.
|
||||
for ( int nMap = 0 ; nMap < m_nMapNum ; ++ nMap) {
|
||||
Point3d ptInfIntBox = b3Int.GetMin() ;
|
||||
Point3d ptSupIntBox = b3Int.GetMax() ;
|
||||
// Dal sistema intrinseco al sistema griglia (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) ;
|
||||
swap( ptInfIntBox.x, ptInfIntBox.z) ;
|
||||
swap( ptInfIntBox.x, ptInfIntBox.y) ;
|
||||
swap( ptSupIntBox.x, ptSupIntBox.z) ;
|
||||
swap( ptSupIntBox.x, ptSupIntBox.y) ;
|
||||
}
|
||||
else if ( nMap == 2) {
|
||||
swap( ptLineSt.x, ptLineSt.y) ;
|
||||
swap( ptLineSt.y, ptLineSt.z) ;
|
||||
swap( vtLineDir.x, vtLineDir.y) ;
|
||||
swap( vtLineDir.y, vtLineDir.z) ;
|
||||
swap( ptInfIntBox.y, ptInfIntBox.z) ;
|
||||
swap( ptInfIntBox.x, ptInfIntBox.y) ;
|
||||
swap( ptSupIntBox.y, ptSupIntBox.z) ;
|
||||
swap( ptSupIntBox.x, ptSupIntBox.y) ;
|
||||
}
|
||||
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) ;
|
||||
// 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.
|
||||
int nI = nDex % m_nNx[nMap] ;
|
||||
int nJ = nDex / m_nNx[nMap] ;
|
||||
// Se fuori dalla regione ammissibile salto l'iterazione
|
||||
if ( nI < nStI || nI > nEnI || nJ < nStJ || nJ > nEnJ)
|
||||
continue ;
|
||||
// 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 == 1) {
|
||||
dMinU = min( ptInt1.x, ptInt2.x) ;
|
||||
dMaxU = max( ptInt1.x, ptInt2.x) ;
|
||||
else if ( nMap == 2) {
|
||||
swap( ptLineSt.x, ptLineSt.y) ;
|
||||
swap( ptLineSt.y, ptLineSt.z) ;
|
||||
swap( vtLineDir.x, vtLineDir.y) ;
|
||||
swap( vtLineDir.y, vtLineDir.z) ;
|
||||
}
|
||||
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 ;
|
||||
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 ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -857,7 +1114,7 @@ VolZmap::AvoidTruncatedCone( const Point3d& ptBase, const Vector3d vtSymmetryAx,
|
||||
//----------------------------------------------------------------------------
|
||||
bool
|
||||
VolZmap::AvoidTruncatedPyramid( const Point3d& ptBase, const Vector3d& vtAxZ, const Vector3d& vtAxX,
|
||||
double dSegTop, double dSegBot, double dHeight, bool bPrecise) const
|
||||
double dSegTop, double dSegBot, double dHeight, double dSafeDist, bool bPrecise) const
|
||||
{
|
||||
// Porto il tronco di piramide nel sistema intrinseco e normalizzo i vettori.
|
||||
Point3d ptMyBase = ptBase ;
|
||||
@@ -869,6 +1126,14 @@ VolZmap::AvoidTruncatedPyramid( const Point3d& ptBase, const Vector3d& vtAxZ, co
|
||||
vtMyAxZ.Normalize() ;
|
||||
vtMyAxX.Normalize() ;
|
||||
|
||||
// Se distanza di sicurezza maggiore di epsilon espando il cono
|
||||
if ( dSafeDist > EPS_SMALL) {
|
||||
ptMyBase -= ( dSafeDist * vtMyAxZ) ;
|
||||
dHeight += ( 2 * dSafeDist) ;
|
||||
dSegTop += ( 2 * dSafeDist) ;
|
||||
dSegBot += ( 2 * dSafeDist) ;
|
||||
}
|
||||
|
||||
// Se i BoundingBox non si sovrappongono, ho finito
|
||||
Vector3d vtMyAxY = vtMyAxZ ^ vtMyAxX ;
|
||||
vtMyAxY.Normalize() ;
|
||||
@@ -882,7 +1147,8 @@ VolZmap::AvoidTruncatedPyramid( const Point3d& ptBase, const Vector3d& vtAxZ, co
|
||||
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()))
|
||||
BBox3d b3Int ;
|
||||
if ( ! b3Zmap.FindIntersection( b3PyramyBox, b3Int))
|
||||
return true ;
|
||||
|
||||
// Definisco un sistema di riferimento con origine nel centro della base maggiore del tronco di piramide,
|
||||
@@ -904,56 +1170,120 @@ VolZmap::AvoidTruncatedPyramid( const Point3d& ptBase, const Vector3d& vtAxZ, co
|
||||
}
|
||||
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 ( ! bPrecise) {
|
||||
// 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 ptLineSt = ORIG + ( i + 0.5) * m_dStep * X_AX + ( j + 0.5) * m_dStep * Y_AX ;
|
||||
if ( k == 0)
|
||||
;
|
||||
else if ( k == 1)
|
||||
ptLineSt += -0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 2)
|
||||
ptLineSt += +0.4 * m_dStep * X_AX - 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 3)
|
||||
ptLineSt += +0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
else if ( k == 4)
|
||||
ptLineSt += -0.4 * m_dStep * X_AX + 0.4 * m_dStep * Y_AX ;
|
||||
Point3d ptInt1, ptInt2 ;
|
||||
if ( IntersLineTruncatedPyramid( ptLineSt, Z_AX, frPyramidFrame, dMinSeg, dMaxSeg, dHeight,
|
||||
ptInt1, Vector3d( 0., 0., 0.), ptInt2, Vector3d( 0., 0., 0.))) {
|
||||
double dZmin = min( ptInt1.z, ptInt2.z) ;
|
||||
double dZmax = max( ptInt1.z, ptInt2.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 ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Ciclo sulle mappe
|
||||
for ( int nMap = 0 ; nMap < m_nMapNum ; ++ nMap) {
|
||||
Point3d ptInfIntBox = b3Int.GetMin();
|
||||
Point3d ptSupIntBox = b3Int.GetMax();
|
||||
// Dal sistema intrinseco al sistema griglia (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) ;
|
||||
swap( ptInfIntBox.x, ptInfIntBox.z) ;
|
||||
swap( ptInfIntBox.x, ptInfIntBox.y) ;
|
||||
swap( ptSupIntBox.x, ptSupIntBox.z) ;
|
||||
swap( ptSupIntBox.x, ptSupIntBox.y) ;
|
||||
}
|
||||
else if ( nMap == 2) {
|
||||
swap( ptLineSt.x, ptLineSt.y) ;
|
||||
swap( ptLineSt.y, ptLineSt.z) ;
|
||||
swap( vtLineDir.x, vtLineDir.y) ;
|
||||
swap( vtLineDir.y, vtLineDir.z) ;
|
||||
swap( ptInfIntBox.y, ptInfIntBox.z) ;
|
||||
swap( ptInfIntBox.x, ptInfIntBox.y) ;
|
||||
swap( ptSupIntBox.y, ptSupIntBox.z) ;
|
||||
swap( ptSupIntBox.x, ptSupIntBox.y) ;
|
||||
}
|
||||
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) ;
|
||||
// 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
|
||||
int nI = nDex % m_nNx[nMap] ;
|
||||
int nJ = nDex / m_nNx[nMap] ;
|
||||
// Se fuori dalla regione ammissibile salto l'iterazione
|
||||
if ( nI < nStI || nI > nEnI || nJ < nStJ || nJ > nEnJ)
|
||||
continue ;
|
||||
// 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 == 1) {
|
||||
dMinU = min( ptInt1.x, ptInt2.x) ;
|
||||
dMaxU = max( ptInt1.x, ptInt2.x) ;
|
||||
else if ( nMap == 2) {
|
||||
swap( ptLineSt.x, ptLineSt.y) ;
|
||||
swap( ptLineSt.y, ptLineSt.z) ;
|
||||
swap( vtLineDir.x, vtLineDir.y) ;
|
||||
swap( vtLineDir.y, vtLineDir.z) ;
|
||||
}
|
||||
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 ;
|
||||
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 ;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1533,18 +1863,21 @@ VolZmap::IntersLineTruncatedPyramid( const Point3d& ptLineSt, const Vector3d& vt
|
||||
if ( abs( vtV.z) < EPS_ZERO && ( ptP.z < EPS_SMALL || ptP.z > dHeight + EPS_SMALL))
|
||||
return false ;
|
||||
|
||||
double dHalfMax = 0.5 * dSegMax ;
|
||||
double dHalfMin = 0.5 * dSegMin ;
|
||||
|
||||
// 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) {
|
||||
if ( abs( ptIntPlaneMax.x) <= dHalfMax && abs( ptIntPlaneMax.y) <= dHalfMax) {
|
||||
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 ( abs( ptIntPlaneMin.x) <= dHalfMin && abs( ptIntPlaneMin.y) <= dHalfMin) {
|
||||
if ( nIntNum == 0) {
|
||||
ptInt1 = ptIntPlaneMin ;
|
||||
vtN1 = Z_AX ;
|
||||
@@ -1571,96 +1904,96 @@ VolZmap::IntersLineTruncatedPyramid( const Point3d& ptLineSt, const Vector3d& vt
|
||||
|
||||
// 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)))
|
||||
( ( ptIntPlaneMax.x < - dHalfMax - EPS_SMALL && ptIntPlaneMin.x < - dHalfMin - EPS_SMALL) ||
|
||||
( ptIntPlaneMax.x > dHalfMax + EPS_SMALL && ptIntPlaneMin.x > dHalfMin + EPS_SMALL) ||
|
||||
( ptIntPlaneMax.y < - dHalfMax - EPS_SMALL && ptIntPlaneMin.y < - dHalfMin - EPS_SMALL) ||
|
||||
( ptIntPlaneMax.y > dHalfMax + EPS_SMALL && ptIntPlaneMin.y > dHalfMin + 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)) ;
|
||||
Point3d ptPlaneP( - dHalfMax, - dHalfMax, 0.) ;
|
||||
Vector3d vtPlaneN( 0., - dHeight, ( dHalfMax - dHalfMin)) ;
|
||||
vtPlaneN.Normalize() ;
|
||||
if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) {
|
||||
if ( nIntNum == 0) {
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ;
|
||||
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)
|
||||
( ptInt1.x + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt1.z &&
|
||||
( ptInt1.x - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt1.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
else {
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ;
|
||||
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)
|
||||
( ptInt2.x + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt2.z &&
|
||||
( ptInt2.x - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt2.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
}
|
||||
vtPlaneN = Vector3d( - dHeight, 0., ( dSegMax - dSegMin)) ;
|
||||
vtPlaneN = Vector3d( - dHeight, 0., ( dHalfMax - dHalfMin)) ;
|
||||
vtPlaneN.Normalize() ;
|
||||
if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) {
|
||||
if ( nIntNum == 0) {
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ;
|
||||
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)
|
||||
( ptInt1.y + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt1.z &&
|
||||
( ptInt1.y - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt1.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
else {
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ;
|
||||
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)
|
||||
( ptInt2.y + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt2.z &&
|
||||
( ptInt2.y - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt2.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
}
|
||||
ptPlaneP = Point3d( dSegMax, dSegMax, 0.) ;
|
||||
vtPlaneN = Vector3d( dHeight, 0., ( dSegMax - dSegMin)) ;
|
||||
ptPlaneP = Point3d( dHalfMax, dHalfMax, 0.) ;
|
||||
vtPlaneN = Vector3d( dHeight, 0., ( dHalfMax - dHalfMin)) ;
|
||||
vtPlaneN.Normalize() ;
|
||||
if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) {
|
||||
if ( nIntNum == 0) {
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ;
|
||||
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)
|
||||
( ptInt1.y + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt1.z &&
|
||||
( ptInt1.y - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt1.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
else {
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ;
|
||||
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)
|
||||
( ptInt2.y + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt2.z &&
|
||||
( ptInt2.y - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt2.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
}
|
||||
vtPlaneN = Vector3d( 0., dHeight, ( dSegMax - dSegMin)) ;
|
||||
vtPlaneN = Vector3d( 0., dHeight, ( dHalfMax - dHalfMin)) ;
|
||||
vtPlaneN.Normalize() ;
|
||||
if ( nIntNum < 2 && abs( vtV * vtPlaneN) > EPS_ZERO) {
|
||||
if ( nIntNum == 0) {
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
ptInt1 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ;
|
||||
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)
|
||||
( ptInt1.x + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt1.z &&
|
||||
( ptInt1.x - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt1.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
else {
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtPlaneN ;
|
||||
ptInt2 = ptP + ( ( ( ptPlaneP - ptP) * vtPlaneN) / ( vtV * vtPlaneN)) * vtV ;
|
||||
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)
|
||||
( ptInt2.x + dHalfMax) * dHeight >= ( dHalfMax - dHalfMin) * ptInt2.z &&
|
||||
( ptInt2.x - dHalfMax) * dHeight <= ( dHalfMin - dHalfMax) * ptInt2.z)
|
||||
++ nIntNum ;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user