Dario Sassi
0dd8afedf9
- correzioni nell'intersezione di archi tangenti o quasi - pulizia uso distanze e relative costanti minime.
8524 lines
315 KiB
C++
8524 lines
315 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2015-2016
|
|
//----------------------------------------------------------------------------
|
|
// File : VolZmap.cpp Data : 22.01.15 Versione : 1.6a4
|
|
// Contenuto : Implementazione della classe Volume Zmap (singola griglia)
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 22.01.15 DS Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
//--------------------------- Include ----------------------------------------
|
|
#include "stdafx.h"
|
|
#include "VolZmap.h"
|
|
#include "GeoConst.h"
|
|
|
|
using namespace std ;
|
|
|
|
//---------- Sottrazione intervalli ------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::SubtractIntervals( unsigned int nI, unsigned int nJ, double dMin, double dMax)
|
|
{
|
|
// Controllo che dMin e dMax non siano quasi coincidenti
|
|
if ( abs( dMax - dMin) < EPS_SMALL)
|
|
return true ;
|
|
|
|
// Controllo che dMin < dMax
|
|
if ( dMax < dMin )
|
|
swap( dMax, dMin) ;
|
|
|
|
// Calcolo nPos dello spillone
|
|
unsigned int nPos = nJ * m_nNx + nI ;
|
|
|
|
// Ciclo sugli intervalli dello spillone
|
|
bool bModified = false ;
|
|
unsigned int i = 0 ;
|
|
while ( i + 1 < m_ZValues[nPos].size()) {
|
|
|
|
// Intervallo da sottrarre è tutto a sinistra di quello corrente, non vi è intersezione
|
|
if ( m_ZValues[nPos][i] > dMax - EPS_SMALL) {
|
|
;
|
|
}
|
|
|
|
// Intersezione
|
|
else if ( m_ZValues[nPos][i + 1] > dMax + EPS_SMALL) {
|
|
// L'intervallo corrente corrente viene limitato a sinistra
|
|
if ( m_ZValues[nPos][i] > dMin - EPS_SMALL) {
|
|
bModified = true ;
|
|
m_ZValues[nPos][i] = dMax ;
|
|
}
|
|
// L'intervallo si divide in due intervalli
|
|
else {
|
|
bModified = true ;
|
|
m_ZValues[nPos].resize( m_ZValues[nPos].size() + 2) ;
|
|
|
|
for ( size_t j = m_ZValues[nPos].size() - 1 ; j >= i + 3 ; -- j)
|
|
|
|
m_ZValues[nPos][j] = m_ZValues[nPos][j - 2] ;
|
|
|
|
m_ZValues[nPos][i + 1] = dMin ;
|
|
m_ZValues[nPos][i + 2] = dMax ;
|
|
|
|
i = i + 2 ;
|
|
}
|
|
}
|
|
|
|
else {
|
|
// L'intervallo corrente viene eliminato
|
|
if ( m_ZValues[nPos][i] > dMin - EPS_SMALL) {
|
|
bModified = true ;
|
|
for ( unsigned int j = i ; j < m_ZValues[nPos].size() - 2 ; ++ j)
|
|
|
|
m_ZValues[nPos][j] = m_ZValues[nPos][j + 2] ;
|
|
|
|
m_ZValues[nPos].resize( m_ZValues[nPos].size() - 2) ;
|
|
|
|
i = i - 2 ;
|
|
}
|
|
// L'intervallo corrente viene limitato a destra
|
|
else if ( m_ZValues[nPos][i + 1] > dMin + EPS_SMALL) {
|
|
bModified = true ;
|
|
m_ZValues[nPos][i + 1] = dMin ;
|
|
}
|
|
// L'intervallo da sottrarre è tutto a destra di quello corrente, non vi è intersezione
|
|
else {
|
|
;
|
|
}
|
|
}
|
|
|
|
i = i + 2 ;
|
|
}
|
|
|
|
// Se eseguita modifica, imposto ricalcolo della grafica
|
|
if ( bModified)
|
|
m_OGrMgr.Reset() ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::SubtractIntervals( const Point3d& ptP, double dMin, double dMax)
|
|
{
|
|
// ptP è espresso nel sistema locale e viene convertito in quello intrinseco (localFrame)
|
|
Point3d ptPL = ptP ;
|
|
ptPL.ToLoc( m_LocalFrame) ;
|
|
|
|
double dX, dY, dZ ; // Coordinate di ptPL nel sistema intrinseco
|
|
double dhMin, dhMax ; // Altezze dMin e dMax RIESPRESSE nel sistema intrinseco (dMin e dMax sono altezze rispetto a ptP)
|
|
|
|
dX = ptPL.x ; dY = ptPL.y ; dZ = ptPL.z ;
|
|
dhMin = dZ + dMin ; dhMax = dZ + dMax ;
|
|
|
|
// Cerco il punto della griglia più vicino (indice è l'intero appena più basso)
|
|
double integerPartX = floor( dX / m_dStep) ;
|
|
double integerPartY = floor( dY / m_dStep) ;
|
|
|
|
unsigned int i = static_cast <unsigned int> (integerPartX) ; // Indici del punto di griglia più vicino.
|
|
unsigned int j = static_cast <unsigned int> (integerPartY) ; // i = 0, 1, ..., m_Nx - 1 ; j = 0, 1, ..., m_Ny - 1
|
|
|
|
// Controllo se le coordinate x e y del punto dato siano all'interno della griglia:
|
|
// se sono dentro la griglia chiamo l'altra subtract
|
|
if ( dX < m_dStep * m_nNx &&
|
|
dY < m_dStep * m_nNy &&
|
|
dX >= 0 && dY >= 0) {
|
|
// Mettendo > - qlc può sempre capitare un punto compreso fra - qlc e 0 e
|
|
// si esce dai limiti dell vector e vi è errore runtime
|
|
return SubtractIntervals( i, j, dhMin, dhMax) ;
|
|
}
|
|
|
|
// altrimenti non succede niente
|
|
return true ;
|
|
}
|
|
|
|
//---------- Addizione intervalli --------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::AddIntervals( unsigned int nI, unsigned int nJ, double dMin, double dMax)
|
|
{
|
|
// Controllo che dMin e dMax non siano quasi coincidenti
|
|
if ( abs( dMax - dMin) < EPS_SMALL)
|
|
return true ;
|
|
|
|
// Controllo che dMin < dMax
|
|
if ( dMax < dMin)
|
|
swap( dMax, dMin) ;
|
|
|
|
// Calcolo nPos
|
|
unsigned int nPos = nJ * m_nNx + nI ;
|
|
|
|
// Se spillone vuoto
|
|
if ( m_ZValues[nPos].size() == 0) {
|
|
|
|
m_ZValues[nPos].resize( 2) ;
|
|
|
|
m_ZValues[nPos][0] = dMin ;
|
|
m_ZValues[nPos][1] = dMax ;
|
|
|
|
m_OGrMgr.Reset() ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
// Ciclo sugli intervalli dello spillone
|
|
bool bModified = false ;
|
|
unsigned int i = 0 ;
|
|
while ( i + 1 < m_ZValues[nPos].size()) {
|
|
|
|
// Eventuale aggiustamento di intervalli sovrapposti
|
|
if ( i > 0) {
|
|
if ( m_ZValues[nPos][i] < m_ZValues[nPos][i - 1] + EPS_SMALL) {
|
|
// Se l'intervallo corrente non è contenuto totalmente si esegue l'istruzione successiva
|
|
if ( m_ZValues[nPos][i - 1] < m_ZValues[nPos][i + 1] + EPS_SMALL)
|
|
|
|
m_ZValues[nPos][i - 1] = m_ZValues[nPos][i + 1] ;
|
|
|
|
for ( unsigned int j = i ; j < m_ZValues[nPos].size() - 2 ; ++ j)
|
|
m_ZValues[nPos][j] = m_ZValues[nPos][j + 2] ;
|
|
|
|
m_ZValues[nPos].resize( m_ZValues[nPos].size() - 2) ;
|
|
|
|
i = i - 2 ;
|
|
}
|
|
}
|
|
|
|
// Caso in cui devo aggiungere un intervallo a sinistra dell'intervallo corrente
|
|
if ( m_ZValues[nPos][i] > dMax + EPS_SMALL) {
|
|
bModified = true ;
|
|
|
|
m_ZValues[nPos].resize( m_ZValues[nPos].size() + 2) ;
|
|
|
|
for ( size_t j = m_ZValues[nPos].size() - 1 ; j >= i + 2 ; -- j)
|
|
m_ZValues[nPos][j] = m_ZValues[nPos][j - 2] ;
|
|
|
|
m_ZValues[nPos][i] = dMin ;
|
|
m_ZValues[nPos][i + 1] = dMax ;
|
|
|
|
i = i + 2 ;
|
|
}
|
|
|
|
// Casi d'intersezione:
|
|
else if ( m_ZValues[nPos][i + 1] > dMax - EPS_SMALL) {
|
|
// Se l'intervallo da aggiungere sconfina a sinistra modifico il minimo dell'intervalo corrente
|
|
if ( m_ZValues[nPos][i] > dMin - EPS_SMALL) {
|
|
bModified = true ;
|
|
m_ZValues[nPos][i] = dMin ;
|
|
}
|
|
}
|
|
|
|
else {
|
|
// Se l'intervallo corrente è tutto contenuto nell'intervallo da aggungere modifico gli estremi
|
|
if ( m_ZValues[nPos][i] > dMin + EPS_SMALL) {
|
|
bModified = true ;
|
|
m_ZValues[nPos][i] = dMin ;
|
|
m_ZValues[nPos][i + 1] = dMax ;
|
|
}
|
|
// Se l'intervallo da aggiungere sconfina a destra modifico il massimo dell'intervallo corrente
|
|
else if ( m_ZValues[nPos][i + 1] > dMin - EPS_SMALL) {
|
|
bModified = true ;
|
|
m_ZValues[nPos][i + 1] = dMax ;
|
|
}
|
|
else {
|
|
// Aggiungo intervallo a destra dell'ultimo intervallo
|
|
if ( i == m_ZValues[nPos].size() - 2) {
|
|
bModified = true ;
|
|
m_ZValues[nPos].resize( m_ZValues[nPos].size() + 2) ;
|
|
|
|
m_ZValues[nPos][i + 2] = dMin ;
|
|
m_ZValues[nPos][i + 3] = dMax ;
|
|
|
|
i = i + 2 ;
|
|
}
|
|
}
|
|
}
|
|
|
|
i = i + 2 ;
|
|
}
|
|
|
|
// se eseguita modifica, imposto ricalcolo della grafica
|
|
if ( bModified)
|
|
m_OGrMgr.Reset() ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::AddIntervals( const Point3d& ptP, double dMin, double dMax)
|
|
{
|
|
// ptP è espresso nel sistema locale e viene convertito in quello intrinseco (localFrame)
|
|
Point3d ptPL = ptP ;
|
|
ptPL.ToLoc( m_LocalFrame) ;
|
|
|
|
double dX, dY, dZ ; // Coordinate di ptPL nel sistema intrinseco
|
|
double dhMin, dhMax ; // Altezze dMin e dMax RIESPRESSE nel sistema intrinseco (dMin e dMax sono altezze rispetto a ptP)
|
|
|
|
dX = ptPL.x ; dY = ptPL.y ; dZ = ptPL.z ;
|
|
dhMin = dZ + dMin ; dhMax = dZ + dMax ;
|
|
|
|
// Cerco il punto della griglia più vicino
|
|
double integerPartX = floor( dX / m_dStep) ;
|
|
double integerPartY = floor( dY / m_dStep) ;
|
|
|
|
unsigned int i = static_cast <unsigned int> (integerPartX) ; // Indici del punto di griglia più vicino.
|
|
unsigned int j = static_cast <unsigned int> (integerPartY) ; // i = 0, 1, ..., m_Nx - 1 ; j = 0, 1, ..., m_Ny - 1
|
|
|
|
// Controllo se le coordinate x e y del punto dato siano all'interno della griglia:
|
|
// se sono dentro la griglia chiamo l'altra subtract
|
|
if ( dX < m_dStep*m_nNx && dY < m_dStep*m_nNy
|
|
&& dX >= 0 && dY >= 0) {
|
|
// Mettendo > - qlc può sempre capitare un punto compreso fra - qlc e 0 e
|
|
// si esce dai limiti dell vector e vi è errore runtime
|
|
return AddIntervals( i, j, dhMin, dhMax) ;
|
|
}
|
|
|
|
// altrimenti non succede niente
|
|
return false ;
|
|
}
|
|
|
|
//---------- Volumi ----------------------------------------------------------
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingStep( const Point3d& ptPs, const Vector3d& vtDs, const Point3d& ptPe, const Vector3d& vtDe)
|
|
{
|
|
// Controllo sull'effettiva esistenza del movimento
|
|
if ( AreSamePointApprox( ptPs, ptPe) && AreSameVectorApprox( vtDs, vtDe))
|
|
return true ;
|
|
|
|
// Porto i dati del movimento nel riferimento intrinseco; quest'operazione è necessaria perché
|
|
// viene chiamata la funzione di sottrazione che accetta come parametri gli indici,
|
|
// non quella che chiede il punto, nella quale viene eseguita la trasformazione di coordinate.
|
|
Point3d ptLs = ptPs ; ptLs.ToLoc( m_LocalFrame) ;
|
|
Point3d ptLe = ptPe ; ptLe.ToLoc( m_LocalFrame) ;
|
|
Vector3d vtLs = vtDs ; vtLs.ToLoc( m_LocalFrame) ;
|
|
Vector3d vtLe = vtDe ; vtLe.ToLoc( m_LocalFrame) ;
|
|
|
|
// Normalizzo i vettori
|
|
if ( ! vtLs.Normalize() || ! vtLe.Normalize())
|
|
return false ;
|
|
|
|
// Direzione utensile costante
|
|
if ( AreSameVectorApprox( vtLs, vtLe)) {
|
|
|
|
// Versori della direzione utensile diretti come Z
|
|
if ( vtLs.LenXY() < EPS_SMALL) {
|
|
|
|
// Movimento diretto come direzione utensile
|
|
if ( AreSamePointXYApprox( ptLs, ptLe)) {
|
|
if ( m_nToolType == GenericTool)
|
|
return DrillZ( ptLs, ptLe, vtLs) ;
|
|
else
|
|
return DrillingZ( ptLs, ptLe, vtLs) ;
|
|
}
|
|
|
|
// Movimento perpendicolare a direzione utensile
|
|
if ( abs( ptLe.z - ptLs.z) < EPS_SMALL) {
|
|
if ( m_nToolType == 0)
|
|
return MillZ( ptLs, ptLe, vtLs) ;
|
|
else
|
|
return MillingPerpZ( ptLs, ptLe, vtLs) ;
|
|
}
|
|
|
|
// Movimento generico
|
|
if ( m_nToolType == 0)
|
|
return MillZ( ptLs, ptLe, vtLs) ;
|
|
else
|
|
return MillingZ( ptLs, ptLe, vtLs) ;
|
|
}
|
|
|
|
// Versori della direzione utensile nel piano XY
|
|
else if ( abs( vtLs.z) < EPS_SMALL) {
|
|
|
|
Vector3d vtDir( vtLs.x, vtLs.y, 0) ; vtDir.Normalize() ;
|
|
|
|
// Movimento con Z costante (con vettore movimento parallelo od ortogonale al versore dell'utensile)
|
|
if ( abs( ptLe.z - ptLs.z) < EPS_SMALL) {
|
|
|
|
Vector3d vtTest( ptLe.x - ptLs.x, ptLe.y - ptLs.y, 0) ;
|
|
Vector3d vtTLong = ( vtTest * vtDir) * vtDir ;
|
|
Vector3d vtTOrt = vtTest - vtTLong ;
|
|
|
|
// Movimento parallelo alla direzione dell'utensile (foratura)
|
|
if ( vtTOrt.IsSmall()) {
|
|
if ( m_nToolType == 0)
|
|
return DrillingGT( ptLs, ptLe, vtDir) ;
|
|
else
|
|
return DrillingXY( ptLs, ptLe, vtDir) ;
|
|
}
|
|
|
|
// Movimento perpendicolare alla direzione dell'utensile
|
|
if ( vtTLong.IsSmall()) {
|
|
if ( m_nToolType == GenericTool)
|
|
return MillingGT( ptLs, ptLe, vtDir) ;
|
|
else
|
|
return MillingPerpXY( ptLs, ptLe, vtDir) ;
|
|
}
|
|
|
|
// Movimento nel piano generico
|
|
if ( m_nToolType == GenericTool)
|
|
return MillingGT( ptLs, ptLe, vtDir) ;
|
|
else
|
|
return MillingXYPlaneGen( ptLs, ptLe, vtDir) ;
|
|
}
|
|
|
|
// Movimento con Z non costante
|
|
else {
|
|
|
|
if ( m_nToolType == GenericTool)
|
|
return MillingGT( ptLs, ptLe, vtDir) ;
|
|
|
|
// Movimento verticale
|
|
if ( AreSamePointXYApprox( ptLs, ptLe))
|
|
return MillingXYVert( ptLs, ptLe, vtDir) ;
|
|
|
|
// Grandezze geometriche per selezione
|
|
Vector3d vtMove = ptLe - ptLs ;
|
|
Vector3d vtTLong = ( vtMove * vtDir) * vtDir ;
|
|
Vector3d vtTOrt = vtMove - ( vtMove * vtDir) * vtDir ;
|
|
|
|
// Movimento LongVert
|
|
if ( vtTOrt.IsSmallXY())
|
|
return MillingXYLongVert( ptLs, ptLe, vtDir) ;
|
|
|
|
// Movimento perpendicolare alla direzione dell'utensile
|
|
if ( vtTLong.IsSmallXY())
|
|
return MillingXY( ptLs, ptLe, vtDir) ;
|
|
|
|
// Movimento generico con versore direzione nel piano
|
|
return Milling( ptLs, ptLe, vtDir) ;
|
|
}
|
|
}
|
|
|
|
// Caso generico
|
|
else {
|
|
|
|
Vector3d vtMove = ptLe - ptLs ;
|
|
Vector3d vtOrt = vtMove - ( vtMove * vtLs) * vtLs ;
|
|
|
|
// Drilling
|
|
if ( vtOrt.IsSmall()) {
|
|
if ( m_nToolType == GenericTool)
|
|
return DrillingGT( ptLs, ptLe, vtLs) ;
|
|
else
|
|
return Drilling( ptLs, ptLe, vtLs) ;
|
|
}
|
|
// Milling
|
|
else {
|
|
if ( m_nToolType == GenericTool)
|
|
return MillingGT( ptLs, ptLe, vtLs) ;
|
|
else
|
|
return Milling( ptLs, ptLe, vtLs) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Altri casi, non gestiti
|
|
return false ;
|
|
}
|
|
|
|
// Versore utensile parallelo all'asse Z
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::DrillingZ( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
// Cilindro sfera toro
|
|
if ( m_nToolType == CylindricalMill || m_nToolType == BallEndMill ||
|
|
m_nToolType == BullNoseMill)
|
|
|
|
return CBTDrillZ( ptLs, ptLe, vtToolDir) ;
|
|
// Coni
|
|
else if ( m_nToolType == ConusMill)
|
|
|
|
return ConusDrillingZ( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else
|
|
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::CBTDrillZ( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
// Bounding box
|
|
double dMinX = min( ptLs.x, ptLe.x) - m_dRadius ;
|
|
double dMinY = min( ptLs.y, ptLe.y) - m_dRadius ;
|
|
double dMaxX = max( ptLs.x, ptLe.x) + m_dRadius ;
|
|
double dMaxY = max( ptLs.y, ptLe.y) + m_dRadius ;
|
|
|
|
// Verifico interferisca con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
|
|
// Determino i limiti sugli indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
// Determino quote estreme del tagliente
|
|
double dMax = ptLs.z - m_dHeight ; // Volendo si può effettuare un controllo su quota punto finale e iniziale,
|
|
double dMin = ptLe.z - m_dHeight ; // ma non è necessaria dal momento che Subtract intervals scambia min con max se max < min
|
|
double dZCutBase = ptLs.z ; // Quota della base del tagliente nella posizione iniziale
|
|
double dDeltaZ = ptLe.z - ptLs.z ; // Differenza delle quote fra le posizioni finale e iniziale della base del tagliente
|
|
|
|
// Limite sul quadrato del raggio
|
|
double dSqRad = ( m_dRadius + EPS_SMALL) * ( m_dRadius + EPS_SMALL) ;
|
|
|
|
// Ciclo sui punti nei limiti
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
double dX = ( i + 0.5) * m_dStep ;
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
double dY = ( j + 0.5) * m_dStep ;
|
|
// punto
|
|
Point3d ptQ( dX, dY, 0) ;
|
|
// determino il quadrato della distanza
|
|
double dSqDist = SqDistXY( ptQ, ptLe) ;
|
|
// se distanza nei limiti, taglio
|
|
if ( dSqDist < dSqRad)
|
|
GetMinMaxZ( i, j, dZCutBase, dDeltaZ, dSqDist, vtToolDir) ;
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::ConusDrillingZ( const Point3d ptLs, const Point3d ptLe, const Vector3d vtToolDir) {
|
|
|
|
double dMinZ = min( min( ptLs.z, ptLs.z - vtToolDir.z * m_dHeight),
|
|
min( ptLe.z, ptLe.z - vtToolDir.z * m_dHeight)) ;
|
|
double dMaxZ = max( max( ptLs.z, ptLs.z - vtToolDir.z * m_dHeight),
|
|
max( ptLe.z, ptLe.z - vtToolDir.z * m_dHeight)) ;
|
|
|
|
// Prima verifica sull'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return true ;
|
|
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
double dMinRad = min( m_dRadius, m_dTipRadius) ;
|
|
|
|
// Bounding box
|
|
double dMinX = ptLs.x - dMaxRad ;
|
|
double dMaxX = ptLs.x + dMaxRad ;
|
|
double dMinY = ptLs.y - dMaxRad ;
|
|
double dMaxY = ptLs.y + dMaxRad ;
|
|
|
|
// Seconda verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
|
|
double dDeltaZ = abs( ptLe.z - ptLs.z) ;
|
|
|
|
Point3d ptO( ptLs.x, ptLs.y, 0) ;
|
|
|
|
// Limiti su indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
|
|
if ( m_dRadius < m_dTipRadius)
|
|
// Coda di rondine
|
|
return GetMinMaxZSw( ptO, nStartI, nEndI, nStartJ, nEndJ, dMinZ, dMaxZ, dMinRad, dMaxRad, vtToolDir.z, dDeltaZ) ;
|
|
// Punta di trapano
|
|
else
|
|
return GetMinMaxZDr( ptO, nStartI, nEndI, nStartJ, nEndJ, dMinZ, dMaxZ, dMinRad, dMaxRad, vtToolDir.z, dDeltaZ) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
inline bool
|
|
VolZmap::GetMinMaxZSw( const Point3d ptO, unsigned int nStartI, unsigned int nEndI,
|
|
unsigned int nStartJ, unsigned int nEndJ, double dMinZ, double dMaxZ,
|
|
double dMinRad, double dMaxRad, double dDir, double dDeltaZ)
|
|
{
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
// Determinazione della posizione xy del dexel rispetto
|
|
// all'asse di simmetria dell'utensile
|
|
double dX = ( i + 0.5) * m_dStep ;
|
|
double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ;
|
|
Vector3d vtC = ptC - ptO ;
|
|
|
|
double dSqDist = vtC * vtC ;
|
|
|
|
// Se il dexel cade nel cerchio di interesse taglio
|
|
if ( dSqDist < dMaxRad * dMaxRad) {
|
|
|
|
double dMin, dMax ;
|
|
|
|
if ( dSqDist < dMinRad * dMinRad) {
|
|
dMin = dMinZ ;
|
|
dMax = dMaxZ ;
|
|
}
|
|
|
|
else {
|
|
double dR = sqrt( dSqDist) ;
|
|
if ( dDir > 0) {
|
|
dMin = dMinZ ;
|
|
dMax = dMinZ + dDeltaZ + ( m_dTipHeight * ( dMaxRad - dR)) / ( dMaxRad - dMinRad) ;
|
|
}
|
|
else {
|
|
dMin = dMaxZ - dDeltaZ + ( m_dTipHeight * ( dR - dMaxRad)) / ( dMaxRad - dMinRad) ;
|
|
dMax = dMaxZ ;
|
|
}
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
inline bool
|
|
VolZmap::GetMinMaxZDr( const Point3d ptO, unsigned int nStartI, unsigned int nEndI,
|
|
unsigned int nStartJ, unsigned int nEndJ, double dMinZ, double dMaxZ,
|
|
double dMinRad, double dMaxRad, double dDir, double dDeltaZ)
|
|
{
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ;
|
|
double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ;
|
|
Vector3d vtC = ptC - ptO ;
|
|
|
|
double dSqDist = vtC * vtC ;
|
|
|
|
// Se il dexel cade nel cerchio di interesse taglio
|
|
if ( dSqDist < dMaxRad * dMaxRad) {
|
|
|
|
double dMin, dMax ;
|
|
|
|
if ( dSqDist < dMinRad * dMinRad) {
|
|
dMin = dMinZ ;
|
|
dMax = dMaxZ ;
|
|
}
|
|
|
|
else {
|
|
double dR = sqrt( dSqDist) ;
|
|
if ( dDir > 0) {
|
|
dMin = dMinZ + ( m_dTipHeight * ( dR - dMinRad)) / ( dMaxRad - dMinRad) ;
|
|
dMax = dMaxZ ;
|
|
}
|
|
else {
|
|
dMin = dMinZ ;
|
|
dMax = dMaxZ + ( m_dTipHeight * ( dR - dMinRad)) / ( dMinRad - dMaxRad) ;
|
|
}
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingPerpZ( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir) {
|
|
// Cilindro sfera toro
|
|
if ( m_nToolType == CylindricalMill ||
|
|
m_nToolType == BallEndMill ||
|
|
m_nToolType == BullNoseMill)
|
|
|
|
return CBTMillingPerpZ( ptLs, ptLe, vtToolDir) ;
|
|
// Coni
|
|
else if ( m_nToolType == ConusMill)
|
|
|
|
return ConusPerpZ( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::CBTMillingPerpZ( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
// Bounding box
|
|
double dMinX = min( ptLs.x, ptLe.x) - m_dRadius ;
|
|
double dMinY = min( ptLs.y, ptLe.y) - m_dRadius ;
|
|
double dMaxX = max( ptLs.x, ptLe.x) + m_dRadius ;
|
|
double dMaxY = max( ptLs.y, ptLe.y) + m_dRadius ;
|
|
|
|
// Verifico interferisca con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
|
|
// Determino i limiti sugli indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
// Determino quote estreme del tagliente
|
|
double dZCutBase = ptLs.z ; // Quota della base del tagliente nella posizione iniziale
|
|
double dDeltaZ = ptLe.z - ptLs.z ; // Differenza delle quote fra le posizioni finale e iniziale della base del tagliente
|
|
|
|
// Limite sul quadrato del raggio
|
|
double dSqRad = ( m_dRadius + EPS_SMALL) * ( m_dRadius + EPS_SMALL) ;
|
|
|
|
// Segmento di movimento (nel piano griglia)
|
|
Point3d ptStart( ptLs.x, ptLs.y, 0) ;
|
|
Point3d ptEnd( ptLe.x, ptLe.y, 0) ;
|
|
double dLen ;
|
|
Vector3d vtDir ;
|
|
DirDist( ptStart, ptEnd, vtDir, dLen) ;
|
|
|
|
// Ciclo sui punti nei limiti
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
double dX = ( i + 0.5) * m_dStep ;
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
double dY = ( j + 0.5) * m_dStep ;
|
|
// punto
|
|
Point3d ptQ( dX, dY, 0) ;
|
|
// determino il quadrato della distanza del punto dal segmento
|
|
double dProiez = vtDir * ( ptQ - ptStart) ;
|
|
if ( dProiez < 0)
|
|
dProiez = 0 ;
|
|
else if ( dProiez > dLen)
|
|
dProiez = dLen ;
|
|
Point3d ptMinDist = ptStart + vtDir * dProiez ;
|
|
double dSqDist = SqDistXY( ptQ, ptMinDist) ;
|
|
// se distanza nei limiti, taglio
|
|
if ( dSqDist < dSqRad)
|
|
GetMinMaxZ( i, j, dZCutBase, dDeltaZ, dSqDist, vtToolDir) ;
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::ConusPerpZ( const Point3d ptLs, const Point3d ptLe, const Vector3d vtToolDir)
|
|
{
|
|
double dMinZ = min( min( ptLs.z, ptLs.z - vtToolDir.z * m_dHeight), min( ptLe.z, ptLe.z - vtToolDir.z * m_dHeight)) ;
|
|
double dMaxZ = max( max( ptLs.z, ptLs.z - vtToolDir.z * m_dHeight), max( ptLe.z, ptLe.z - vtToolDir.z * m_dHeight)) ;
|
|
|
|
// Prima verifica sull'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return true ;
|
|
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
double dMinRad = min( m_dRadius, m_dTipRadius) ;
|
|
|
|
// Bounding box
|
|
double dMinX = min( ptLs.x, ptLe.x) - dMaxRad ;
|
|
double dMaxX = max( ptLs.x, ptLe.x) + dMaxRad ;
|
|
double dMinY = min( ptLs.y, ptLe.y) - dMaxRad ;
|
|
double dMaxY = max( ptLs.y, ptLe.y) + dMaxRad ; // Ricordati del caso balordo da mettere nella documentazione
|
|
|
|
// Seconda verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
|
|
Point3d ptO( ptLs.x, ptLs.y, 0) ;
|
|
Vector3d vtMove = ptLe - ptLs ; double dLen = vtMove.LenXY() ;
|
|
Vector3d vtV1 = vtMove ; vtV1.Normalize() ;
|
|
|
|
double dZBase = ptLs.z ;
|
|
double dZStem = ptLs.z - ( m_dHeight - m_dTipHeight) * vtToolDir.z ;
|
|
double dZTip = ptLs.z - m_dHeight * vtToolDir.z ;
|
|
|
|
// Limiti su indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dMin, dMax ;
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptO ;
|
|
|
|
double dProj1 = vtC * vtV1 ;
|
|
Vector3d vtOrt = vtC - dProj1 * vtV1 ;
|
|
|
|
if ( dProj1 < 0) {
|
|
|
|
double dSqDist = vtC * vtC ;
|
|
|
|
if ( dSqDist < dMinRad * dMinRad) {
|
|
|
|
dMin = min( dZBase, dZTip) ;
|
|
dMax = max( dZBase, dZTip) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
else if ( dSqDist >= dMinRad * dMinRad && dSqDist < dMaxRad * dMaxRad) {
|
|
|
|
double dr = sqrt( dSqDist) ;
|
|
|
|
if ( m_dRadius < m_dTipRadius) {
|
|
|
|
dMin = min( dZTip, dZStem + ( dr - dMinRad) * ( dZTip - dZStem) / ( dMaxRad - dMinRad)) ;
|
|
dMax = max( dZTip, dZStem + ( dr - dMinRad) * ( dZTip - dZStem) / ( dMaxRad - dMinRad)) ;
|
|
}
|
|
else {
|
|
dMin = min( dZBase, dZTip + ( dr - dMinRad) * ( dZStem - dZTip) / ( dMaxRad - dMinRad)) ;
|
|
dMax = max( dZBase, dZTip + ( dr - dMinRad) * ( dZStem - dZTip) / ( dMaxRad - dMinRad)) ;
|
|
}
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
else if ( dProj1 >= 0 && dProj1 < dLen) {
|
|
|
|
double dSqDist = vtOrt * vtOrt ;
|
|
|
|
if ( dSqDist < dMinRad * dMinRad) {
|
|
|
|
dMin = min( dZBase, dZTip) ;
|
|
dMax = max( dZBase, dZTip) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
else if ( dSqDist >= dMinRad * dMinRad && dSqDist < dMaxRad * dMaxRad) {
|
|
|
|
double dr = sqrt( dSqDist) ;
|
|
|
|
if ( m_dRadius < m_dTipRadius) {
|
|
dMin = min( dZTip, dZStem + ( dr - dMinRad) * ( dZTip - dZStem) / ( dMaxRad - dMinRad)) ;
|
|
dMax = max( dZTip, dZStem + ( dr - dMinRad) * ( dZTip - dZStem) / ( dMaxRad - dMinRad)) ;
|
|
}
|
|
|
|
else {
|
|
dMin = min( dZBase, dZTip + ( dr - dMinRad) * ( dZStem - dZTip) / ( dMaxRad - dMinRad)) ;
|
|
dMax = max( dZBase, dZTip + ( dr - dMinRad) * ( dZStem - dZTip) / ( dMaxRad - dMinRad)) ;
|
|
}
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
else if ( dProj1 >= dLen) {
|
|
|
|
Vector3d vtCn = vtC - vtMove ;
|
|
|
|
double dSqDist = vtCn * vtCn ;
|
|
|
|
if ( dSqDist < dMinRad * dMinRad) {
|
|
dMin = min( dZBase, dZTip) ;
|
|
dMax = max( dZBase, dZTip) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
else if ( dSqDist >= dMinRad * dMinRad && dSqDist < dMaxRad * dMaxRad) {
|
|
|
|
double dr = sqrt( dSqDist) ;
|
|
|
|
if ( m_dRadius < m_dTipRadius) {
|
|
dMin = min( dZTip, dZStem + ( dr - dMinRad) * ( dZTip - dZStem) / ( dMaxRad - dMinRad)) ;
|
|
dMax = max( dZTip, dZStem + ( dr - dMinRad) * ( dZTip - dZStem) / ( dMaxRad - dMinRad)) ;
|
|
}
|
|
|
|
else {
|
|
dMin = min( dZBase, dZTip + ( dr - dMinRad) * ( dZStem - dZTip) / ( dMaxRad - dMinRad)) ;
|
|
dMax = max( dZBase, dZTip + ( dr - dMinRad) * ( dZStem - dZTip) / ( dMaxRad - dMinRad)) ;
|
|
}
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
inline bool
|
|
VolZmap::GetMinMaxZ( unsigned int nI, unsigned int nJ, double dZCutBase,
|
|
double dDeltaZ, double dSqDist, const Vector3d& vtToolDir)
|
|
{
|
|
// Definisco variabili
|
|
double dSqRad = m_dRadius * m_dRadius ;
|
|
double dFactor = ( vtToolDir.z < 0 ? - 1 : 1) ;
|
|
double dZtip = ( vtToolDir.z < 0 ? dZCutBase + m_dHeight : dZCutBase - m_dHeight) ;
|
|
|
|
// Caso utensile generico al momento non gestito
|
|
if ( m_nToolType == 0)
|
|
return false ;
|
|
|
|
// Caso Cylindrical Mill
|
|
else if ( m_nToolType == 1) {
|
|
double dMin, dMax ;
|
|
|
|
if ( abs(vtToolDir.z * dDeltaZ) < EPS_SMALL) { // Non è meglio fare if (vt * delta < - Eps_small ) else if ( vt * delta < Eps) else ?
|
|
dMin = min(dZCutBase, dZtip) ;
|
|
dMax = max(dZCutBase, dZtip) ;
|
|
}
|
|
else if ( vtToolDir.z * dDeltaZ < 0) {
|
|
dMin = min(dZtip, dZtip + dDeltaZ) ;
|
|
dMax = max(dZtip, dZtip + dDeltaZ) ;
|
|
}
|
|
else {
|
|
dMin = min(dZCutBase, dZCutBase + dDeltaZ) ;
|
|
dMax = max(dZCutBase, dZCutBase + dDeltaZ) ;
|
|
}
|
|
return SubtractIntervals( nI, nJ, dMin, dMax) ;
|
|
}
|
|
|
|
// Caso Ball-End Mill
|
|
else if ( m_nToolType == 2) {
|
|
double dMin, dMax ;
|
|
|
|
double dH = sqrt( dSqRad - dSqDist) ;
|
|
|
|
if ( abs( vtToolDir.z * dDeltaZ) < EPS_SMALL) {
|
|
dMin = min( dZCutBase, dZtip + dFactor*( m_dRadius - dH)) ;
|
|
dMax = max( dZCutBase, dZtip + dFactor*( m_dRadius - dH)) ;
|
|
}
|
|
else if ( vtToolDir.z * dDeltaZ < 0) {
|
|
dMin = min( dZtip + dFactor*( m_dRadius - dH),
|
|
dZtip + dFactor*( m_dRadius - dH) + dDeltaZ) ;// ocio
|
|
dMax = max( dZtip + dFactor*( m_dRadius - dH),
|
|
dZtip + dFactor*( m_dRadius - dH) + dDeltaZ) ;// ocio
|
|
}
|
|
else {
|
|
dMin = min( dZCutBase, dZCutBase + dDeltaZ) ;
|
|
dMax = max( dZCutBase, dZCutBase + dDeltaZ) ;
|
|
}
|
|
return SubtractIntervals( nI, nJ, dMin, dMax) ;
|
|
}
|
|
|
|
// Caso Bull-Nose Mill
|
|
else if ( m_nToolType == 3) {
|
|
|
|
double dDeltaR = m_dRadius - m_dRCorner ;
|
|
|
|
if ( dSqDist < dDeltaR*dDeltaR) {
|
|
|
|
double dMin, dMax ;
|
|
|
|
if ( abs( vtToolDir.z * dDeltaZ) < EPS_SMALL) {
|
|
dMin = min( dZCutBase, dZtip) ;
|
|
dMax = max( dZCutBase, dZtip) ;
|
|
}
|
|
else if ( vtToolDir.z * dDeltaZ < 0) {
|
|
dMin = min( dZtip, dZtip + dDeltaZ) ;
|
|
dMax = max( dZtip, dZtip + dDeltaZ) ;
|
|
}
|
|
else {
|
|
dMin = min( dZCutBase, dZCutBase + dDeltaZ) ;
|
|
dMax = max( dZCutBase, dZCutBase + dDeltaZ) ;
|
|
}
|
|
return SubtractIntervals( nI, nJ, dMin, dMax) ;
|
|
}
|
|
|
|
else {
|
|
|
|
double dSqRadC = m_dRCorner * m_dRCorner ;
|
|
double dSqd = dSqDist + dDeltaR * dDeltaR - 2 * sqrt( dSqDist) * dDeltaR ;
|
|
double dSqrt = sqrt(dSqRadC - dSqd) ;
|
|
|
|
double dMin, dMax ;
|
|
if ( abs( vtToolDir.z * dDeltaZ) < EPS_SMALL) {
|
|
dMin = min( dZCutBase, dZtip + dFactor*( m_dRCorner - dSqrt)) ;
|
|
dMax = max( dZCutBase, dZtip + dFactor*( m_dRCorner - dSqrt)) ;
|
|
}
|
|
else if ( vtToolDir.z * dDeltaZ < 0) {
|
|
dMin = min( dZtip + dFactor *( m_dRCorner - dSqrt),
|
|
dZtip + dFactor *( m_dRCorner - dSqrt) + dDeltaZ) ;
|
|
dMax = max( dZtip + dFactor *( m_dRCorner - dSqrt),
|
|
dZtip + dFactor *( m_dRCorner - dSqrt) + dDeltaZ) ;
|
|
}
|
|
else {
|
|
dMin = min( dZCutBase, dZCutBase + dDeltaZ) ;
|
|
dMax = max( dZCutBase, dZCutBase + dDeltaZ) ;
|
|
}
|
|
return SubtractIntervals( nI, nJ, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
// Caso di utensile inesistente ( m_nToolType fuori dai valori concessi)
|
|
else
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingZ( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
|
|
if ( m_nToolType == 1 || m_nToolType == 2)
|
|
|
|
return CBMillingZ( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else if ( m_nToolType == 3)
|
|
|
|
return false ;
|
|
|
|
else if ( m_nToolType == 4) {
|
|
|
|
if ( m_dRadius > m_dTipRadius)
|
|
return ConusMillingZDr( ptLs, ptLe, vtToolDir) ;
|
|
else
|
|
return ConusMillingZSw( ptLs, ptLe, vtToolDir) ;
|
|
}
|
|
|
|
else
|
|
return true ; // forse ci va il nuovo
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::CBMillingZ( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
// Setto il fattore per l'orientazione in z
|
|
double dFactor = ( vtToolDir.z < 0 ? 1 : - 1) ;
|
|
|
|
// Bounding box
|
|
double dMinX = min( ptLs.x, ptLe.x) - m_dRadius ;
|
|
double dMaxX = max( ptLs.x, ptLe.x) + m_dRadius ;
|
|
double dMinY = min( ptLs.y, ptLe.y) - m_dRadius ;
|
|
double dMaxY = max( ptLs.y, ptLe.y) + m_dRadius ;
|
|
double dMinZ = min( min( ptLs.z, ptLs.z + dFactor * m_dHeight),
|
|
min( ptLe.z, ptLe.z + dFactor * m_dHeight)) ;
|
|
double dMaxZ = max( max( ptLs.z, ptLs.z + dFactor * m_dHeight),
|
|
max( ptLe.z, ptLe.z + dFactor * m_dHeight)) ;
|
|
|
|
// Verifico interferisca con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return true ;
|
|
|
|
// Determino i limiti sugli indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
//
|
|
double dZCutBase = ptLs.z ; // Quota della base del tagliente nella posizione iniziale
|
|
double dDeltaZ = ptLe.z - ptLs.z ; // Differenza delle quote fra le posizioni finale e iniziale della base del tagliente
|
|
|
|
// Limite sul quadrato del raggio
|
|
double dSqRad = ( m_dRadius + EPS_SMALL) * ( m_dRadius + EPS_SMALL) ;
|
|
|
|
// Segmento di movimento (nel piano griglia)
|
|
Point3d ptStart( ptLs.x, ptLs.y, 0) ;
|
|
Point3d ptEnd( ptLe.x, ptLe.y, 0) ;
|
|
double dLen ;
|
|
Vector3d vtDir ;
|
|
DirDist( ptStart, ptEnd, vtDir, dLen) ;
|
|
|
|
// Ciclo sui punti nei limiti
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
double dX = ( i + 0.5) * m_dStep ;
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
double dY = ( j + 0.5) * m_dStep ;
|
|
// punto
|
|
Point3d ptQ( dX, dY, 0) ;
|
|
// determino il quadrato della distanza del punto dal segmento
|
|
double dProj = vtDir * ( ptQ - ptStart) ;
|
|
double dProiez ;
|
|
|
|
if ( dProj < 0)
|
|
dProiez = 0 ;
|
|
else if ( dProj < dLen)
|
|
dProiez = dProj ;
|
|
else
|
|
dProiez = dLen ;
|
|
|
|
Point3d ptMinDist = ptStart + vtDir * dProiez ;
|
|
double dSqDist = SqDistXY( ptQ, ptMinDist) ;
|
|
|
|
// se distanza nei limiti, taglio
|
|
if ( dSqDist < dSqRad)
|
|
GetMinMaxZGen( i, j, dProj, dSqDist, dLen, dZCutBase, dDeltaZ, vtToolDir) ;
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::ConusMillingZDr( const Point3d ptLs, const Point3d ptLe, const Vector3d vtToolDir)
|
|
{
|
|
double dMinZ = min( min( ptLs.z, ptLs.z - vtToolDir.z * m_dHeight),
|
|
min( ptLe.z, ptLe.z - vtToolDir.z * m_dHeight)) ;
|
|
double dMaxZ = max( max( ptLs.z, ptLs.z - vtToolDir.z * m_dHeight),
|
|
max( ptLe.z, ptLe.z - vtToolDir.z * m_dHeight)) ;
|
|
|
|
// Prima verifica sull'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return true ;
|
|
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
double dMinRad = min( m_dRadius, m_dTipRadius) ;
|
|
|
|
// Bounding box
|
|
double dMinX = min( ptLs.x, ptLe.x) - dMaxRad ;
|
|
double dMaxX = max( ptLs.x, ptLe.x) + dMaxRad ;
|
|
double dMinY = min( ptLs.y, ptLe.y) - dMaxRad ;
|
|
double dMaxY = max( ptLs.y, ptLe.y) + dMaxRad ;
|
|
|
|
// Seconda verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
|
|
// Punti iniziale e finale e proiezione sul piano del punto iniziale
|
|
Point3d ptI, ptF, ptO ;
|
|
|
|
if ( vtToolDir.z > 0) {
|
|
ptI = ( ptLs.z < ptLe.z ? ptLs : ptLe) ; ptO.x = ptI.x ; ptO.y = ptI.y ; ptO.z = 0 ;
|
|
ptF = ( ptLs.z < ptLe.z ? ptLe : ptLs) ;
|
|
}
|
|
else {
|
|
ptI = ( ptLs.z < ptLe.z ? ptLe : ptLs) ; ptO.x = ptI.x ; ptO.y = ptI.y ; ptO.z = 0 ;
|
|
ptF = ( ptLs.z < ptLe.z ? ptLs : ptLe) ;
|
|
}
|
|
|
|
// Quote iniziale e finale della base dell'utensile e DeltaZ
|
|
double dZI = ptI.z ; double dZF = ptF.z ; double dDeltaZ = dZF - dZI ;
|
|
|
|
// Vettori di movimento
|
|
Vector3d vtMove = ptF - ptI ;
|
|
Vector3d vtMoveXY( vtMove.x, vtMove.y, 0) ; double dLen = vtMoveXY.LenXY() ;
|
|
|
|
// Sistema di riferimento sul cono e vertice del cono
|
|
Vector3d vtV1 = vtToolDir ;
|
|
Vector3d vtV2 = vtMoveXY ; vtV2.Normalize() ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
Point3d ptV = ptI - ( m_dHeight + m_dTipHeight * dMinRad / ( dMaxRad - dMinRad)) * vtV1 ;
|
|
|
|
// Apertura del cono e parametri per determinare i piani
|
|
double dTanAlpha = ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dRatio = ( vtMove * vtV1) / ( vtMove * vtV2) ;
|
|
|
|
double dCos = dTanAlpha * dRatio ;
|
|
double dSin = ( abs( dCos) < 1 ? sqrt( 1 - dCos * dCos) : 0) ;
|
|
|
|
double dDen = sqrt( 1 + dTanAlpha * dTanAlpha) ;
|
|
|
|
// Versori normali e prodotti scalari per per determinare i piani
|
|
Vector3d vtNs = - ( dTanAlpha / dDen) * vtV1 +
|
|
( dCos / dDen) * vtV2 +
|
|
( dSin / dDen) * vtV3 ;
|
|
Vector3d vtNd = - ( dTanAlpha / dDen) * vtV1 +
|
|
( dCos / dDen) * vtV2 -
|
|
( dSin / dDen) * vtV3 ;
|
|
|
|
Vector3d vtR0 = ptV - ORIG ;
|
|
double dDots = vtR0 * vtNs ;
|
|
double dDotd = vtR0 * vtNd ;
|
|
|
|
// Limiti su indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
// Grandezze per determinare la configurazione geometrica dell'utensile
|
|
double dMin, dMax ;
|
|
double dX = ( i + 0.5) * m_dStep ;
|
|
double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ;
|
|
Vector3d vtC = ptC - ptO ;
|
|
Vector3d vtCf = vtC - vtMoveXY ;
|
|
|
|
Vector3d vtUC = vtC ; vtUC.Normalize() ;
|
|
Vector3d vtUCf = vtCf ; vtUCf.Normalize() ;
|
|
|
|
double dCCos = vtUC * vtV2 ;
|
|
double dCCosf = vtUCf * vtV2 ;
|
|
|
|
double dProj = vtC * vtV2 ;
|
|
Vector3d vtOrt = vtC - dProj * vtV2 ;
|
|
|
|
double dSqDistI = vtC * vtC ;
|
|
double dSqDistM = vtOrt * vtOrt ;
|
|
double dSqDistF = vtCf * vtCf ;
|
|
|
|
// Se dentro la zona interessata dalla lavorazione valuto
|
|
// la tipologia di tale zona
|
|
if ( ( dProj < 0 && dSqDistI < dMaxRad * dMaxRad) ||
|
|
( dProj >= 0 && dProj < dLen && dSqDistM < dMaxRad * dMaxRad) ||
|
|
( dProj >= dLen && dSqDistF < dMaxRad * dMaxRad)) {
|
|
|
|
// Caso vettore utensile equiverso all'asse Z
|
|
if ( vtToolDir.z > 0) {
|
|
// Massimi
|
|
double dPMaxI = ( dMaxRad * dMaxRad - dSqDistM > 0 ? sqrt( dMaxRad * dMaxRad - dSqDistM) : 0) ;
|
|
|
|
if ( dProj < dLen - dPMaxI)
|
|
|
|
dMax = dZI + ( dDeltaZ / dLen) * ( dProj + dPMaxI) ;
|
|
else
|
|
dMax = dZF ;
|
|
|
|
// Minimi
|
|
if ( dSqDistI < dMinRad * dMinRad)
|
|
|
|
dMin = dZI - m_dHeight ;
|
|
|
|
else {
|
|
|
|
if ( ( vtMove * vtV1) / ( vtMove * vtV2) <= 1 / dTanAlpha) {
|
|
|
|
if ( dSqDistI < dMaxRad * dMaxRad && dCCos < dCos)
|
|
|
|
dMin = dZI - m_dHeight + ( ( sqrt( dSqDistI) - dMinRad) * m_dTipHeight) / ( dMaxRad - dMinRad) ;
|
|
|
|
else if ( dCCos >= dCos && dCCosf < dCos && dSqDistM < dMaxRad * dMaxRad * ( 1 - dCos * dCos)) {
|
|
|
|
double dMinSql = dMinRad * dMinRad * ( 1 - dCos * dCos) ;
|
|
double dMaxSql = dMaxRad * dMaxRad * ( 1 - dCos * dCos) ;
|
|
double dPMinI = ( dMinRad * dMinRad - dSqDistM > 0 ? sqrt( dMinRad * dMinRad - dSqDistM) : 0) ;
|
|
|
|
if ( dSqDistM <= dMinSql)
|
|
|
|
dMin = dZI - m_dHeight + ( dProj - dPMinI) * dDeltaZ / dLen ;
|
|
|
|
else if ( dSqDistM < dMaxSql) {
|
|
|
|
if ( vtC * vtV3 > 0)
|
|
|
|
dMin = ( dDots - dX * vtNs.x - dY * vtNs.y) / vtNs.z ;
|
|
else
|
|
dMin = ( dDotd - dX * vtNd.x - dY * vtNd.y) / vtNd.z ;
|
|
}
|
|
}
|
|
else if ( dCCosf >= dCos) {
|
|
|
|
double dPMinI = ( dMinRad * dMinRad - dSqDistM > 0 ? sqrt( dMinRad * dMinRad - dSqDistM) : 0) ;
|
|
|
|
if ( dSqDistF < dMinRad * dMinRad)
|
|
|
|
dMin = dZI - m_dHeight + ( dProj - dPMinI) * dDeltaZ / dLen ;
|
|
else
|
|
dMin = dZF - m_dHeight + ( ( sqrt( dSqDistF) - dMinRad) * m_dTipHeight) / ( dMaxRad - dMinRad) ;
|
|
}
|
|
else
|
|
|
|
dMin = dZI - m_dHeight + m_dTipHeight + ( dProj - dPMaxI) * dDeltaZ / dLen ;
|
|
}
|
|
else {
|
|
|
|
if ( dSqDistI < dMaxRad * dMaxRad)
|
|
|
|
dMin = dZI - m_dHeight + ( sqrt( dSqDistI) - dMinRad) * m_dTipHeight / ( dMaxRad - dMinRad) ;
|
|
|
|
else if ( dProj >= dPMaxI && dProj < dPMaxI + dLen)
|
|
|
|
dMin = dZI - m_dHeight + m_dTipHeight + ( dProj - dPMaxI) * dDeltaZ / dLen ;
|
|
}
|
|
}
|
|
}
|
|
// Caso vettore utensile opposto all'asse Z
|
|
else {
|
|
// Massimi
|
|
double dPMaxI = ( dMaxRad * dMaxRad - dSqDistM > 0 ? sqrt( dMaxRad * dMaxRad - dSqDistM) : 0) ;
|
|
|
|
if ( dSqDistI < dMinRad * dMinRad)
|
|
|
|
dMax = dZI + m_dHeight ;
|
|
|
|
else {
|
|
|
|
if ( - ( vtMove * vtV1) / ( vtMove * vtV2) <= 1 / dTanAlpha) {
|
|
|
|
if ( dSqDistI < dMaxRad * dMaxRad && dCCos < dCos)
|
|
|
|
dMax = dZI + m_dHeight - ( sqrt( dSqDistI) - dMinRad) * m_dTipHeight / ( dMaxRad - dMinRad) ;
|
|
|
|
else if ( dCCos >= dCos && dCCosf < dCos && dSqDistM < dMaxRad * dMaxRad * ( 1 - dCos * dCos)) {
|
|
|
|
double dMinSql = dMinRad * dMinRad * ( 1 - dCos * dCos) ;
|
|
double dMaxSql = dMaxRad * dMaxRad * ( 1 - dCos * dCos) ;
|
|
double dPMinI = ( dMinRad * dMinRad - dSqDistM > 0 ? sqrt( dMinRad * dMinRad - dSqDistM) : 0) ;
|
|
|
|
if ( dSqDistM <= dMinSql)
|
|
|
|
dMax = dZI + m_dHeight + ( dProj - dPMinI) * dDeltaZ / dLen ;
|
|
|
|
else if ( dSqDistM < dMaxSql) {
|
|
|
|
if ( vtC * vtV3 > 0)
|
|
|
|
dMax = ( dDots - dX * vtNs.x - dY * vtNs.y) / vtNs.z ;
|
|
else
|
|
dMax = ( dDotd - dX * vtNd.x - dY * vtNd.y) / vtNd.z ;
|
|
}
|
|
}
|
|
else if ( dCCosf >= dCos) {
|
|
|
|
double dPMinI = ( dMinRad * dMinRad - dSqDistM > 0 ? sqrt( dMinRad * dMinRad - dSqDistM) : 0) ;
|
|
|
|
if ( dSqDistF < dMinRad * dMinRad)
|
|
|
|
dMax = dZI + m_dHeight + ( dProj - dPMinI) * dDeltaZ / dLen ;
|
|
else
|
|
dMax = dZF + m_dHeight - ( ( sqrt( dSqDistF) - dMinRad) * m_dTipHeight) / ( dMaxRad - dMinRad) ;
|
|
}
|
|
else
|
|
|
|
dMax = dZI + m_dHeight - m_dTipHeight + ( dProj - dPMaxI) * dDeltaZ / dLen ;
|
|
|
|
}
|
|
else {
|
|
|
|
if ( dSqDistI < dMaxRad * dMaxRad)
|
|
|
|
dMax = dZI + m_dHeight - ( ( sqrt( dSqDistI) - dMinRad) * m_dTipHeight) / ( dMaxRad - dMinRad) ;
|
|
|
|
else if ( dProj >= dPMaxI && dProj < dPMaxI + dLen)
|
|
|
|
dMax = dZI + m_dHeight - m_dTipHeight + ( dProj - dPMaxI) * dDeltaZ / dLen ;
|
|
|
|
}
|
|
}
|
|
// Minimi
|
|
double dPMaxCirc = sqrt( dMaxRad * dMaxRad - dSqDistM) ;
|
|
|
|
if ( dProj > dLen - dPMaxCirc)
|
|
dMin = dZF ;
|
|
else
|
|
dMin = dZI + ( dDeltaZ / dLen) * ( dProj + dPMaxCirc) ;
|
|
}
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::ConusMillingZSw( const Point3d ptLs, const Point3d ptLe, const Vector3d vtToolDir)
|
|
{
|
|
double dMinZ = min( min( ptLs.z, ptLs.z - vtToolDir.z * m_dHeight),
|
|
min( ptLe.z, ptLe.z - vtToolDir.z * m_dHeight)) ;
|
|
double dMaxZ = max( max( ptLs.z, ptLs.z - vtToolDir.z * m_dHeight),
|
|
max( ptLe.z, ptLe.z - vtToolDir.z * m_dHeight)) ;
|
|
|
|
// Prima verifica sull'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return true ;
|
|
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
double dMinRad = min( m_dRadius, m_dTipRadius) ;
|
|
|
|
// Bounding box
|
|
double dMinX = min( ptLs.x, ptLe.x) - dMaxRad ;
|
|
double dMaxX = max( ptLs.x, ptLe.x) + dMaxRad ;
|
|
double dMinY = min( ptLs.y, ptLe.y) - dMaxRad ;
|
|
double dMaxY = max( ptLs.y, ptLe.y) + dMaxRad ;
|
|
|
|
// Seconda verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
|
|
// Punti iniziale e finale e proiezione sul piano del punto iniziale
|
|
Point3d ptI, ptF, ptO ;
|
|
|
|
if ( vtToolDir.z > 0) {
|
|
|
|
ptI = ( ptLs.z < ptLe.z ? ptLe : ptLs) ; ptO.x = ptI.x ; ptO.y = ptI.y ; ptO.z = 0 ;
|
|
ptF = ( ptLs.z < ptLe.z ? ptLs : ptLe) ;
|
|
}
|
|
|
|
else {
|
|
ptI = ( ptLs.z < ptLe.z ? ptLs : ptLe) ; ptO.x = ptI.x ; ptO.y = ptI.y ; ptO.z = 0 ;
|
|
ptF = ( ptLs.z < ptLe.z ? ptLe : ptLs) ;
|
|
}
|
|
|
|
Point3d ptICyl = ( ptLs.z < ptLe.z ? ptLs : ptLe) ;
|
|
|
|
double dDeltaH = m_dHeight - m_dTipHeight ;
|
|
double dMinZCyl = min( ptICyl.z, ptICyl.z - vtToolDir.z * dDeltaH) ;
|
|
|
|
// Punti contatto cono cilindro
|
|
Point3d ptIS = ptI - ( m_dHeight - m_dTipHeight) * vtToolDir ;
|
|
Point3d ptFS = ptF - ( m_dHeight - m_dTipHeight) * vtToolDir ;
|
|
|
|
// Quote iniziali e finali e DeltaZ
|
|
double dZI = ptI.z ; double dZF = ptF.z ;
|
|
double dDeltaZ = dZF - dZI ; double dADeltaZ = abs( dDeltaZ) ;
|
|
double dZIS = ptIS.z ; double dZFS = ptFS.z ;
|
|
|
|
// Vettori di movimento
|
|
Vector3d vtMove = ptF - ptI ;
|
|
Vector3d vtMoveXY( vtMove.x, vtMove.y, 0) ; double dLen = vtMoveXY.LenXY() ;
|
|
|
|
// Sistema di riferimento sul cono e vertice del cono
|
|
Vector3d vtV1 = - vtToolDir ;
|
|
Vector3d vtV2 = vtMoveXY ; vtV2.Normalize() ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
Point3d ptV = ptI + ( m_dHeight - m_dTipHeight * ( 1 + dMinRad / ( dMaxRad - dMinRad))) * vtV1 ;
|
|
|
|
// Apertura del cono e parametri per determinare i piani
|
|
double dTanAlpha = ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dRatio = ( vtMove * vtV1) / ( vtMove * vtV2) ;
|
|
|
|
double dCos = dTanAlpha * dRatio ;
|
|
double dSin = ( abs( dCos) < 1 ? sqrt( 1- dCos * dCos) : 0) ;
|
|
|
|
double dDen = sqrt( 1 + dTanAlpha * dTanAlpha) ;
|
|
|
|
// Versori normali e prodotti scalari per per determinare i piani
|
|
Vector3d vtNp = - ( dTanAlpha / dDen) * vtV1 +
|
|
( dCos / dDen) * vtV2 +
|
|
( dSin / dDen) * vtV3 ;
|
|
Vector3d vtNm = - ( dTanAlpha / dDen) * vtV1 +
|
|
( dCos / dDen) * vtV2 -
|
|
( dSin / dDen) * vtV3 ;
|
|
|
|
Vector3d vtR0 = ptV - ORIG ;
|
|
double dDotp = vtR0 * vtNp ;
|
|
double dDotm = vtR0 * vtNm ;
|
|
|
|
// Limiti su indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++j) {
|
|
|
|
double dMin, dMax ;
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptO ; Vector3d vtCf = vtC - vtMoveXY ;
|
|
|
|
Vector3d vtUC = vtC ; Vector3d vtUCf = vtCf ; vtUC.Normalize() ; vtUCf.Normalize() ;
|
|
|
|
double dCCos = vtUC * vtV2 ; double dCCosf = vtUCf * vtV2 ;
|
|
|
|
double dProj = vtC * vtV2 ;
|
|
Vector3d vtOrt = vtC - dProj * vtV2 ;
|
|
|
|
double dSqDistI = vtC * vtC ;
|
|
double dSqDistM = vtOrt * vtOrt ;
|
|
double dSqDistF = vtCf * vtCf ;
|
|
|
|
if ( ( dProj < 0 && dSqDistI < dMaxRad * dMaxRad) ||
|
|
( dProj >= 0 && dProj < dLen && dSqDistM < dMaxRad * dMaxRad) ||
|
|
( dProj >= dLen && dSqDistF < dMaxRad * dMaxRad)) {
|
|
// Caso vettore utensile equiverso all'asse Z
|
|
if ( vtV1.z < 0) {
|
|
|
|
double dPMaxI = ( dMaxRad * dMaxRad - dSqDistM > 0 ? sqrt( dMaxRad * dMaxRad - dSqDistM) : 0) ;
|
|
|
|
// Massimi
|
|
if ( dRatio <= 1 / dTanAlpha) {
|
|
|
|
if ( dSqDistI < dMinRad * dMinRad)
|
|
|
|
dMax = dZIS ;
|
|
|
|
else {
|
|
|
|
if ( dSqDistI < dMaxRad * dMaxRad && dCCos < dCos)
|
|
|
|
dMax = dZIS - ( sqrt( dSqDistI) - dMinRad) * m_dTipHeight / ( dMaxRad - dMinRad) ;
|
|
|
|
else if ( dCCos >= dCos && dCCosf < dCos && dSqDistM < dMaxRad * dMaxRad * ( 1 - dCos * dCos)) {
|
|
|
|
double dMinSql = dMinRad * dMinRad * ( 1 - dCos * dCos) ;
|
|
double dMaxSql = dMaxRad * dMaxRad * ( 1 - dCos * dCos) ;
|
|
double dPMinI = ( dMinRad * dMinRad - dSqDistM > 0 ? sqrt( dMinRad * dMinRad - dSqDistM) : 0) ;
|
|
|
|
if ( dSqDistM <= dMinSql)
|
|
|
|
dMax = dZIS + ( dProj - dPMinI) * dDeltaZ / dLen ;
|
|
|
|
else if ( dSqDistM < dMaxSql) {
|
|
|
|
if ( vtC * vtV3 > 0)
|
|
|
|
dMax = ( dDotp - dX * vtNp.x - dY * vtNp.y) / vtNp.z ;
|
|
else
|
|
dMax = ( dDotm - dX * vtNm.x - dY * vtNm.y) / vtNm.z ;
|
|
}
|
|
}
|
|
else if ( dCCosf >= dCos) {
|
|
|
|
double dPMinI = ( dMinRad * dMinRad - dSqDistM > 0 ? sqrt( dMinRad * dMinRad - dSqDistM) : 0) ;
|
|
|
|
if ( dSqDistF < dMinRad * dMinRad)
|
|
|
|
dMax = dZIS + ( dProj - dPMinI) * dDeltaZ / dLen ;
|
|
else
|
|
dMax = dZFS - ( ( sqrt( dSqDistF) - dMinRad) * m_dTipHeight) / ( dMaxRad - dMinRad) ;
|
|
}
|
|
else
|
|
dMax = dZIS - m_dTipHeight + ( dProj - dPMaxI) * dDeltaZ / dLen ;
|
|
}
|
|
}
|
|
else {
|
|
|
|
if ( dSqDistI < dMinRad * dMinRad)
|
|
|
|
dMax = dZIS ;
|
|
|
|
else if ( dSqDistI < dMaxRad * dMaxRad)
|
|
|
|
dMax = dZIS - ( sqrt( dSqDistI) - dMinRad) * m_dTipHeight / ( dMaxRad - dMinRad) ;
|
|
|
|
else {
|
|
|
|
if ( dProj >= dPMaxI)
|
|
|
|
dMax = dZIS - m_dTipHeight + ( dProj - dPMaxI) * dDeltaZ / dLen ;
|
|
}
|
|
}
|
|
// Minimi
|
|
|
|
if ( dSqDistF < dMaxRad * dMaxRad)
|
|
|
|
dMin = dZFS - m_dTipHeight ;
|
|
|
|
else if ( dProj <= dLen - dPMaxI)
|
|
|
|
dMin = dZIS - m_dTipHeight + ( dProj + dPMaxI) * dDeltaZ / dLen ;
|
|
|
|
}
|
|
// Caso vettore utensile opposto all'asse Z
|
|
else {
|
|
|
|
double dPMaxI = ( dMaxRad * dMaxRad - dSqDistM > 0 ? sqrt( dMaxRad * dMaxRad - dSqDistM) : 0) ;
|
|
|
|
// Massimi
|
|
if ( dSqDistF < dMaxRad * dMaxRad)
|
|
|
|
dMax = dZFS + m_dTipHeight ;
|
|
|
|
else if ( dProj <= dLen - dPMaxI)
|
|
|
|
dMax = dZIS + m_dTipHeight + ( dProj + dPMaxI) * dDeltaZ / dLen ;
|
|
|
|
// Minimi
|
|
if ( dRatio <= 1 / dTanAlpha) {
|
|
|
|
if ( dSqDistI < dMinRad * dMinRad)
|
|
|
|
dMin = dZIS ;
|
|
|
|
else {
|
|
|
|
if ( dSqDistI < dMaxRad * dMaxRad && dCCos < dCos)
|
|
|
|
dMin = dZIS + ( sqrt( dSqDistI) - dMinRad) * m_dTipHeight / ( dMaxRad - dMinRad) ;
|
|
|
|
else if ( dCCos >= dCos && dCCosf < dCos && dSqDistM < dMaxRad * dMaxRad * ( 1 - dCos * dCos)) {
|
|
|
|
double dMinSql = dMinRad * dMinRad * ( 1 - dCos * dCos) ;
|
|
double dMaxSql = dMaxRad * dMaxRad * ( 1 - dCos * dCos) ;
|
|
double dPMinI = ( dMinRad * dMinRad - dSqDistM > 0 ? sqrt( dMinRad * dMinRad - dSqDistM) : 0) ;
|
|
|
|
if ( dSqDistM <= dMinSql)
|
|
|
|
dMin = dZIS + ( dProj - dPMinI) * dDeltaZ / dLen ;
|
|
|
|
else if ( dSqDistM < dMaxSql) {
|
|
|
|
if ( vtC * vtV3 > 0)
|
|
|
|
dMin = ( dDotp - dX * vtNp.x - dY * vtNp.y) / vtNp.z ;
|
|
else
|
|
dMin = ( dDotm - dX * vtNm.x - dY * vtNm.y) / vtNm.z ;
|
|
}
|
|
}
|
|
else if ( dCCosf >= dCos) {
|
|
|
|
double dPMinI = ( dMinRad * dMinRad - dSqDistM > 0 ? sqrt( dMinRad * dMinRad - dSqDistM) : 0) ;
|
|
|
|
if ( dSqDistF < dMinRad * dMinRad)
|
|
|
|
dMin = dZIS + ( dProj - dPMinI) * dDeltaZ / dLen ;
|
|
else
|
|
dMin = dZFS + ( ( sqrt( dSqDistF) - dMinRad) * m_dTipHeight) / ( dMaxRad - dMinRad) ;
|
|
}
|
|
else
|
|
dMin = dZIS + m_dTipHeight + ( dProj - dPMaxI) * dDeltaZ / dLen ;
|
|
}
|
|
}
|
|
else {
|
|
|
|
if ( dSqDistI < dMinRad * dMinRad)
|
|
|
|
dMin = dZIS ;
|
|
|
|
else if ( dSqDistI < dMaxRad * dMaxRad)
|
|
|
|
dMin = dZIS + ( sqrt( dSqDistI) - dMinRad) * m_dTipHeight / ( dMaxRad - dMinRad) ;
|
|
|
|
else
|
|
|
|
dMin = dZIS + m_dTipHeight + ( dProj - dPMaxI) * dDeltaZ / dLen ;
|
|
|
|
}
|
|
}
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
// Parte cilindrica
|
|
Vector3d vtCyl = ( vtV1.z < 0 ? vtCf : vtC) ;
|
|
Vector3d vtCylf = ( vtV1.z < 0 ? vtC : vtCf) ;
|
|
Vector3d vtMot = ( vtV1.z < 0 ? - vtV2 : vtV2) ;
|
|
|
|
double dCylProj = vtCyl * vtMot ;
|
|
double dCylSqDistI = vtCyl * vtCyl ;
|
|
double dCylSqDistF = vtCylf * vtCylf ;
|
|
double dCylSqDistM = ( vtCyl - dCylProj * vtMot) * ( vtCyl - dCylProj * vtMot) ;
|
|
|
|
|
|
if ( dCylSqDistI < dMinRad * dMinRad
|
|
|| dCylSqDistF < dMinRad * dMinRad
|
|
|| ( dCylProj > 0 && dCylProj < dLen && dCylSqDistM < dMinRad * dMinRad)) {
|
|
|
|
double dSt = sqrt( dMinRad * dMinRad - dCylSqDistM) ;
|
|
|
|
// Minimi
|
|
if ( dCylSqDistI < dMinRad * dMinRad )
|
|
|
|
dMin = dMinZCyl ;
|
|
|
|
else if ( dCylProj >= dSt)
|
|
|
|
dMin = dMinZCyl + ( dCylProj - dSt) * dADeltaZ / dLen ;
|
|
|
|
// Massimi
|
|
if ( dCylSqDistF < dMinRad * dMinRad)
|
|
|
|
dMax = dMinZCyl + dDeltaH + dADeltaZ ;
|
|
|
|
else if ( dCylProj <= dLen - dSt)
|
|
|
|
dMax = dMinZCyl + dDeltaH + ( dCylProj + dSt) * dADeltaZ / dLen ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
|
|
}
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
inline bool
|
|
VolZmap::GetMinMaxZGen( unsigned int nI, unsigned int nJ, double dProj, double dSqd,
|
|
double dLenPath, double dZheight, double dDelta, const Vector3d& vtToolDir)
|
|
{
|
|
// Controllo sul tipo di utensile: se 0 utensile generico (al momento non gestito) se maggiore di 3 utensile fuori dai tipi consentiti
|
|
if ( m_nToolType == 0 || m_nToolType > 3)
|
|
return false ;
|
|
|
|
// Definisco variabili quota della punta, minimo e massimo dell'intervallo da sottrarre
|
|
double dZTip = ( vtToolDir.z < 0 ? dZheight + m_dHeight : dZheight - m_dHeight) ;
|
|
double dMin, dMax ; double dStart ; // dStart è un parametro che esprime l'ascissa in cui la retta congiungente le due posizioni iniziale e finale
|
|
double dRSqDist1, dRSqDist2 ; // di un punto del tagliente (nel sistema dell'asse dell'utensile nella posizione iniziale) assume la quota dZheight
|
|
|
|
// Nei conti è comodo che dSqd assuma sempre il significato di distanza del punto dall'asse del movimento al quadrato
|
|
if ( dProj < 0)
|
|
dSqd = dSqd - dProj * dProj ;
|
|
else if ( dProj > dLenPath)
|
|
dSqd = dSqd - ( dProj - dLenPath) * ( dProj - dLenPath) ;
|
|
|
|
// Caso di cylindrical mill
|
|
if ( m_nToolType == 1) {
|
|
|
|
double dZ1, dZ2 ;
|
|
// Se lavora la punta
|
|
if ( vtToolDir.z * dDelta < 0) {
|
|
dZ1 = dZTip ;
|
|
dZ2 = dZheight ;
|
|
}
|
|
// Se lavora il fondo
|
|
else {
|
|
dZ1 = dZheight ;
|
|
dZ2 = dZTip ;
|
|
}
|
|
|
|
dStart = sqrt( m_dRadius * m_dRadius - dSqd) ;
|
|
dRSqDist1 = dProj * dProj + dSqd ;
|
|
dRSqDist2 = ( dLenPath - dProj) * ( dLenPath - dProj) + dSqd ;
|
|
|
|
if ( dRSqDist1 < (m_dRadius + EPS_SMALL) * (m_dRadius + EPS_SMALL)) {
|
|
|
|
dMin = min( max( dZ1 + ( dDelta * ( dProj + dStart)) / dLenPath, dZ1 + dDelta), dZ2) ;
|
|
dMax = max( min( dZ1 + ( dDelta * ( dProj + dStart)) / dLenPath, dZ1 + dDelta), dZ2) ;
|
|
}
|
|
else if ( dRSqDist2 < (m_dRadius + EPS_SMALL) * (m_dRadius + EPS_SMALL)) {
|
|
|
|
dMin = min( min(dZ2 + ( dDelta * ( dProj - dStart)) / dLenPath, dZ2 + dDelta), dZ1 + dDelta) ;
|
|
dMax = max( max(dZ2 + ( dDelta * ( dProj - dStart)) / dLenPath, dZ2 + dDelta), dZ1 + dDelta) ;
|
|
}
|
|
else {
|
|
|
|
dMin = min( dZ1 + ( dDelta * ( dProj + dStart)) / dLenPath, dZ2 + ( dDelta * ( dProj - dStart)) / dLenPath) ;
|
|
dMax = max( dZ1 + ( dDelta * ( dProj + dStart)) / dLenPath, dZ2 + ( dDelta * ( dProj - dStart)) / dLenPath) ;
|
|
}
|
|
}
|
|
|
|
// Caso di ball-end mill
|
|
else if ( m_nToolType == 2) {
|
|
|
|
if ( dDelta < 0) {
|
|
dDelta = - dDelta ;
|
|
dProj = dLenPath - dProj ;
|
|
dZheight = dZheight - dDelta ;
|
|
dZTip = dZTip - dDelta ;
|
|
}
|
|
|
|
dStart = sqrt( m_dRadius * m_dRadius - dSqd) ;
|
|
dRSqDist1 = dProj * dProj + dSqd ;
|
|
dRSqDist2 = ( dLenPath - dProj) * ( dLenPath - dProj) + dSqd ;
|
|
|
|
if ( vtToolDir.z > 0) {
|
|
|
|
// Semi-asse ellisse
|
|
double dSemiAxMin = m_dRadius * sqrt( 1 - dLenPath * dLenPath / ( dLenPath * dLenPath + dDelta * dDelta)) ;
|
|
double dSqrSemiAxMin = m_dRadius * m_dRadius * ( 1 - dLenPath * dLenPath / ( dLenPath * dLenPath + dDelta * dDelta)) ;
|
|
double dXD2 = ( dProj - dLenPath) * ( dProj - dLenPath) ;
|
|
|
|
if ( dRSqDist2 < m_dRadius * m_dRadius) {
|
|
|
|
dMax = dZheight + dDelta ;
|
|
|
|
double dTest = ( 1 - dSqd / ( m_dRadius * m_dRadius) > 0 ? sqrt( 1 - dSqd / ( m_dRadius * m_dRadius)) : 0) ;
|
|
|
|
if ( dProj - dLenPath > dSemiAxMin * dTest) {
|
|
|
|
double dSqrRad = ( dProj - dLenPath) * ( dProj - dLenPath) + dSqd ;
|
|
double dH = ( m_dRadius * m_dRadius - dSqrRad > 0 ? sqrt( m_dRadius * m_dRadius - dSqrRad) : 0) ;
|
|
dMin = dZTip + m_dRadius + dDelta - dH ;
|
|
}
|
|
else if ( dProj > dSemiAxMin * dTest) {
|
|
// Determino l'altezza del punto sull'ellisse da cui passa la retta
|
|
double dPr0 = ( ( 1 - dSqd / ( m_dRadius * m_dRadius)) > 0 ? dLenPath + dSemiAxMin * sqrt( ( 1 - dSqd / (m_dRadius * m_dRadius))) : dLenPath) ;
|
|
double dPar = ( m_dRadius * m_dRadius - dSqrSemiAxMin > 0 ? sqrt( m_dRadius * m_dRadius - dSqrSemiAxMin) : 0) ;
|
|
double dZ0 = ( ( dPr0 - dLenPath) * dPar) / dSemiAxMin ;
|
|
|
|
dMin = dZTip + dDelta + m_dRadius - dZ0 + ( dDelta / dLenPath) * ( dProj - dPr0) ;
|
|
}
|
|
else {
|
|
|
|
double dSqrRad = dProj * dProj + dSqd ;
|
|
double dH = ( m_dRadius * m_dRadius - dSqrRad > 0 ? sqrt( m_dRadius * m_dRadius - dSqrRad) : 0) ;
|
|
dMin = dZTip + m_dRadius - dH ;
|
|
}
|
|
}
|
|
else {
|
|
|
|
dMax = dZheight + ( dDelta * ( dProj + dStart)) / dLenPath ;
|
|
|
|
double dTest = ( 1 - dSqd / ( m_dRadius * m_dRadius) > 0 ? sqrt( 1 - dSqd / ( m_dRadius * m_dRadius)) : 0) ;
|
|
|
|
if ( dProj > dSemiAxMin * dTest) {
|
|
// Determino l'altezza del punto sull'ellisse da cui passa la retta
|
|
double dPr0 = ( ( 1 - dSqd / ( m_dRadius * m_dRadius)) > 0 ? dLenPath + dSemiAxMin * sqrt( ( 1 - dSqd / (m_dRadius * m_dRadius))) : dLenPath) ;
|
|
double dPar = ( m_dRadius * m_dRadius - dSqrSemiAxMin > 0 ? sqrt( m_dRadius * m_dRadius - dSqrSemiAxMin) : 0) ;
|
|
double dZ0 = ( ( dPr0 - dLenPath) * dPar) / dSemiAxMin ;
|
|
|
|
dMin = dZTip + dDelta + m_dRadius - dZ0 + ( dDelta / dLenPath) * ( dProj - dPr0) ;
|
|
}
|
|
else {
|
|
|
|
double dSqrRad = dProj * dProj + dSqd ;
|
|
double dH = ( m_dRadius * m_dRadius - dSqrRad > 0 ? sqrt( m_dRadius * m_dRadius - dSqrRad) : 0) ;
|
|
|
|
dMin = dZTip + m_dRadius - dH ;
|
|
}
|
|
}
|
|
}
|
|
|
|
else {
|
|
|
|
// Semi-asse ellisse
|
|
double dSemiAxMin = m_dRadius * sqrt( 1 - dLenPath * dLenPath / ( dLenPath * dLenPath + dDelta * dDelta)) ;
|
|
double dSqrSemiAxMin = m_dRadius * m_dRadius * ( 1 - dLenPath * dLenPath / ( dLenPath * dLenPath + dDelta * dDelta)) ;
|
|
double dXD2 = ( dProj - dLenPath) * ( dProj - dLenPath) ;
|
|
|
|
if ( dRSqDist1 < m_dRadius * m_dRadius) {
|
|
|
|
dMin = dZheight ;
|
|
|
|
double dTest = ( 1 - dSqd / ( m_dRadius * m_dRadius) > 0 ? sqrt( 1 - dSqd / ( m_dRadius * m_dRadius)) : 0) ;
|
|
|
|
if ( dProj < - dSemiAxMin * dTest) {
|
|
|
|
double dSqrRad = dProj * dProj + dSqd ;
|
|
double dH = ( m_dRadius * m_dRadius - dSqrRad > 0 ? sqrt( m_dRadius * m_dRadius - dSqrRad) : 0) ;
|
|
|
|
dMax = dZTip - m_dRadius + dH ;
|
|
}
|
|
else if ( dProj - dLenPath < - dSemiAxMin * dTest) {
|
|
// Determino l'altezza del punto sull'ellisse da cui passa la retta
|
|
double dPr0 = ( ( 1 - dSqd / ( m_dRadius * m_dRadius)) > 0 ? dSemiAxMin * sqrt( ( 1 - dSqd / (m_dRadius * m_dRadius))) : 0) ;
|
|
double dPar = ( m_dRadius * m_dRadius - dSqrSemiAxMin > 0 ? sqrt( m_dRadius * m_dRadius - dSqrSemiAxMin) : 0) ;
|
|
double dZ0 = ( dPr0 * dPar) / dSemiAxMin ;
|
|
|
|
dMax = dZTip - m_dRadius + dZ0 + ( dDelta / dLenPath) * ( dProj + dPr0) ;
|
|
}
|
|
else {
|
|
|
|
double dSqrRad = dProj * dProj + dSqd ;
|
|
double dH = ( m_dRadius * m_dRadius - dSqrRad > 0 ? sqrt( m_dRadius * m_dRadius - dSqrRad) : 0) ;
|
|
dMax = dZTip - m_dRadius + dDelta + dH ;
|
|
}
|
|
}
|
|
else {
|
|
|
|
dMin = dZheight + ( dDelta * ( dProj - dStart)) / dLenPath ;
|
|
|
|
double dTest = ( 1 - dSqd / ( m_dRadius * m_dRadius) > 0 ? sqrt( 1 - dSqd / ( m_dRadius * m_dRadius)) : 0) ;
|
|
|
|
if ( dProj - dLenPath < - dSemiAxMin * dTest) {
|
|
// Determino l'altezza del punto sull'ellisse da cui passa la retta
|
|
double dPr0 = ( ( 1 - dSqd / ( m_dRadius * m_dRadius)) > 0 ? dSemiAxMin * sqrt( ( 1 - dSqd / (m_dRadius * m_dRadius))) : 0) ;
|
|
double dPar = ( m_dRadius * m_dRadius - dSqrSemiAxMin > 0 ? sqrt( m_dRadius * m_dRadius - dSqrSemiAxMin) : 0) ;
|
|
double dZ0 = ( dPr0 * dPar) / dSemiAxMin ;
|
|
|
|
dMax = dZTip - m_dRadius + dZ0 + ( dDelta / dLenPath) * ( dProj + dPr0) ;
|
|
}
|
|
else {
|
|
|
|
double dSqrRad = ( dProj - dLenPath) * ( dProj - dLenPath) + dSqd ;
|
|
double dH = ( m_dRadius * m_dRadius - dSqrRad > 0 ? sqrt( m_dRadius * m_dRadius - dSqrRad) : 0) ;
|
|
dMax = dZTip + dDelta - m_dRadius + dH ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// Caso di bull-nose mill
|
|
else
|
|
return true ;
|
|
|
|
return SubtractIntervals( nI, nJ, dMin, dMax) ;
|
|
}
|
|
|
|
|
|
// Versore utensile nel piano XY
|
|
|
|
// DeltaZ = 0
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::DrillingXY( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
|
|
if ( m_nToolType == CylindricalMill ||
|
|
m_nToolType == BallEndMill ||
|
|
m_nToolType == BullNoseMill)
|
|
|
|
return CBTDrillXY( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else if ( m_nToolType == ConusMill)
|
|
|
|
return ConusDrillingXY( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else
|
|
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::CBTDrillXY( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
if ( m_nToolType == ConusMill)
|
|
return ConusDrillingXY( ptLs, ptLe, vtToolDir) ;
|
|
|
|
// Punti per la determinazione del materiale asportato
|
|
Vector3d vtMove = ptLe - ptLs ;
|
|
// Punti di riferimento dell'asportazione
|
|
Point3d ptLNs ;
|
|
Point3d ptLNe ;
|
|
// Parametro relativo all'utensile altezza della parte non cilindrica e fattore determinante
|
|
// la lunghezza della parte lavorata a seconda che lavori la punta o il fondo
|
|
double dCylH ; double dFactor ;
|
|
|
|
// Caso utensile generico (al momento non gestito)
|
|
if ( m_nToolType == 0)
|
|
return false ;
|
|
// Caso Cylindrical Mill
|
|
else if ( m_nToolType == 1)
|
|
dCylH = m_dHeight ;
|
|
// Caso Ball-end Mill
|
|
else if ( m_nToolType == 2)
|
|
dCylH = m_dHeight - m_dRadius ;
|
|
// Caso Bull-nose Mill
|
|
else if ( m_nToolType == 3)
|
|
dCylH = m_dHeight - m_dRCorner ;
|
|
|
|
// Normalizzo tale vettore e ne determino la lunghezza:
|
|
double dLenPath = vtMove.Len() ; vtMove.Normalize() ;
|
|
|
|
// Prodotto scalare fra versore direzione utensile e direzione movimento
|
|
double dScProd = vtMove * vtToolDir ;
|
|
// Se lavora la punta
|
|
if ( dScProd < 0) {
|
|
// Trovo i punti di riferimento per la lavorazione
|
|
ptLNs = ptLs + ( vtMove * dCylH) ;
|
|
ptLNe = ptLe + ( vtMove * dCylH) ;
|
|
dFactor = 1 ;
|
|
}
|
|
// Se lavora il fondo
|
|
else {
|
|
ptLNs = ptLs ;
|
|
ptLNe = ptLe ;
|
|
dFactor = 0 ;
|
|
}
|
|
|
|
// Quota z dei punti iniziale e finale
|
|
double dHz = ptLNs.z ;
|
|
|
|
// Bounding box
|
|
double dMinX = min( ptLNs.x, ptLNe.x) - m_dRadius ;
|
|
double dMaxX = max( ptLNs.x, ptLNe.x) + m_dRadius ;
|
|
double dMinY = min( ptLNs.y, ptLNe.y) - m_dRadius ;
|
|
double dMaxY = max( ptLNs.y, ptLNe.y) + m_dRadius ;
|
|
double dMinZ = dHz - m_dRadius ;
|
|
double dMaxZ = dHz + m_dRadius ;
|
|
|
|
// Verifico se il movimento intersca lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return true ;
|
|
|
|
// Determino i limiti sugli indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
//Proietto ptLNs sul piano XY:
|
|
Point3d ptStart( ptLNs.x, ptLNs.y, 0) ;
|
|
|
|
// Ciclo sui punti
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
// Punto da valutare
|
|
Point3d ptC( (i + 0.5)*m_dStep, (j + 0.5)*m_dStep, 0) ;
|
|
// Vettore spostamento da ptLe a ptC
|
|
Vector3d vtC = ( ptC - ptStart) ;
|
|
// Componenti parallela e perpendicolare a vtMove
|
|
// Vettore ortogonale a vtMove
|
|
Vector3d vtOrt = vtMove ;
|
|
// Ruoto vtOrt affinché sia ortogonale
|
|
vtOrt.Rotate( Z_AX, -90) ;
|
|
|
|
double dProj = vtC * vtOrt ;
|
|
Vector3d vtPara = vtOrt * dProj ; // Parallelo alla perpendicolare al movimento
|
|
Vector3d vtPerp = vtC - vtPara ; // Perpendicolare alla perpendicolare al movimento ( serve per unire le getminmax di drill e perp)
|
|
// Distanza di ptC dall'asse dell'untensile
|
|
double dSqDist = vtPerp.SqLen() ;
|
|
double dLimitMill = dLenPath + dFactor * ( m_dHeight - dCylH) ;
|
|
// Se dTestProj è positivo è vtC è dalla parte giusta
|
|
double dTestProj = vtC * vtMove ;
|
|
|
|
if ( dTestProj > 0 && dSqDist < dLimitMill * dLimitMill) {
|
|
if ( dProj > - m_dRadius && dProj < m_dRadius)
|
|
GetMinMaxXY( i, j, dProj, dHz, dSqDist, 0, dLenPath, dScProd) ;
|
|
}
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::ConusDrillingXY( const Point3d ptLs, const Point3d ptLe, const Vector3d vtToolDir)
|
|
{
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
double dMin, dMax ;
|
|
|
|
// Determinazione dell'interferenza dell'utensile con lo Zmap, bouding box e limiti su indici
|
|
BoundingBox( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ) ;
|
|
|
|
// Parametri geometrici dell'utensile
|
|
double dMinRad = min( m_dRadius, m_dTipRadius) ;
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
|
|
Vector3d vtMove = ptLe - ptLs ; double dLen = vtMove.LenXY() ; vtMove.Normalize() ;
|
|
|
|
Point3d ptI = ( vtToolDir * vtMove < 0 ? ptLs : ptLe) ; double dZH = ptI.z ;
|
|
|
|
Vector3d vtV1 = vtToolDir ;
|
|
Vector3d vtV2 = vtV1 ; vtV2.Rotate( Z_AX, 90) ;
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep , dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptI ;
|
|
|
|
double dPL = vtC * vtV1 ;
|
|
double dPT = vtC * vtV2 ;
|
|
|
|
if ( m_dRadius > m_dTipRadius) {
|
|
|
|
if ( dPL < 0 && dPL > - dCylH - dLen && dPT > - dMaxRad && dPT < dMaxRad) {
|
|
|
|
double dH = sqrt( dMaxRad * dMaxRad - dPT * dPT) ;
|
|
|
|
dMin = dZH - dH ; dMax = dZH + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dPL <= - dCylH - dLen && dPL > - m_dHeight - dLen &&
|
|
dPT > - dMaxRad + ( - dPL - dCylH - dLen) * ( dMaxRad - dMinRad) / m_dTipHeight &&
|
|
dPT < dMaxRad - ( - dPL - dCylH - dLen) * ( dMaxRad - dMinRad) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad - ( - dPL - dCylH - dLen) * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
|
|
double dH = sqrt( dr * dr - dPT * dPT) ;
|
|
|
|
dMin = dZH - dH ; dMax = dZH + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else {
|
|
|
|
if ( dPL < 0 && dPL > - dCylH && dPT > - dMinRad && dPT < dMinRad) {
|
|
|
|
double dH = sqrt( dMinRad * dMinRad - dPT * dPT) ;
|
|
|
|
dMin = dZH - dH ; dMax = dZH + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dPL <= - dCylH && dPL > - m_dHeight &&
|
|
dPT > - dMinRad - ( - dPL - dCylH) * ( dMaxRad - dMinRad) / m_dTipHeight &&
|
|
dPT < dMinRad + ( - dPL - dCylH) * ( dMaxRad - dMinRad) / m_dTipHeight) {
|
|
|
|
double dr = dMinRad + ( - dPL - dCylH) * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
|
|
double dH = sqrt( dr * dr - dPT * dPT) ;
|
|
|
|
dMin = dZH - dH ; dMax = dZH + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dPL <= - m_dHeight && dPL > - m_dHeight - dLen && dPT > - dMaxRad && dPT < dMaxRad) {
|
|
|
|
double dH = sqrt( dMaxRad * dMaxRad - dPT * dPT) ;
|
|
|
|
dMin = dZH - dH ; dMax = dZH + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingPerpXY( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
if ( m_nToolType == CylindricalMill ||
|
|
m_nToolType == BallEndMill ||
|
|
m_nToolType == BullNoseMill)
|
|
|
|
return CBTPerpXY( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else if ( m_nToolType == ConusMill)
|
|
|
|
return ConusPerpXY( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else
|
|
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::CBTPerpXY( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
|
|
if ( m_nToolType == ConusMill)
|
|
return ConusPerpXY( ptLs, ptLe, vtToolDir) ;
|
|
|
|
// Determinazione delle posizioni iniziali e finali della punta dell'utensile
|
|
Point3d ptTLs = ptLs - vtToolDir * m_dHeight ;
|
|
Point3d ptTLe = ptLe - vtToolDir * m_dHeight ;
|
|
|
|
// Quota Z
|
|
double dZH = ptLs.z ;
|
|
|
|
// Estremi Bounding box
|
|
double dMinX = min( min( ptLs.x, ptLe.x), min( ptTLs.x, ptTLe.x)) - m_dRadius ;
|
|
double dMaxX = max( max( ptLs.x, ptLe.x), max( ptTLs.x, ptTLe.x)) + m_dRadius ;
|
|
double dMinY = min( min( ptLs.y, ptLe.y), min( ptTLs.y, ptTLe.y)) - m_dRadius ;
|
|
double dMaxY = max( max( ptLs.y, ptLe.y), max( ptTLs.y, ptTLe.y)) + m_dRadius ;
|
|
double dMinZ = dZH - m_dRadius ;
|
|
double dMaxZ = dZH + m_dRadius ;
|
|
|
|
// Verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return true ;
|
|
|
|
// Determinazione limiti sugli indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
// Proiezione sul piano XY delle grandezze di interesse
|
|
Point3d ptStart( ptLs.x, ptLs.y, 0) ;
|
|
Point3d ptEnd( ptLe.x, ptLe.y, 0) ;
|
|
Vector3d vtMove = ptEnd - ptStart ;
|
|
double dLenPath = vtMove.Len() ;
|
|
// Normalizzo il vettore vtMove congiungente le posizioni iniziale e finale della base dell'utensile
|
|
vtMove.Normalize() ;
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
// Punto su cui ciclare e vettore congiungente posizione iniziale della base a tale punto
|
|
Point3d ptC( (i + 0.5) * m_dStep, (j + 0.5) * m_dStep, 0) ;
|
|
Vector3d vtC = ptC - ptStart ;
|
|
// Proiezione di vtC sulla direzione del movimento
|
|
double dProj = vtC * vtMove ;
|
|
// Componente di vtC ortogonale al movimento
|
|
Vector3d vtPerp = vtC - vtMove * dProj ;
|
|
// Lunghezza quadrata del precedente vettore
|
|
double dSqDist = vtPerp.SqLen() ;
|
|
|
|
if ( dProj > - m_dRadius && dProj < dLenPath + m_dRadius && vtPerp * vtToolDir < 0 && dSqDist < m_dHeight * m_dHeight)
|
|
GetMinMaxXY( i, j, dProj, dZH, dSqDist, dLenPath, 0, 0) ;
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::ConusPerpXY( const Point3d ptLs, const Point3d ptLe, const Vector3d vtToolDir)
|
|
{
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
double dMin, dMax ;
|
|
|
|
// Determinazione dell'interferenza dell'utensile con lo Zmap, bouding box e limiti su indici
|
|
bool bControl = BoundingBox( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ) ;
|
|
|
|
if ( bControl == false)
|
|
return true ;
|
|
|
|
// Parametri geometrici dell'utensile
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
|
|
Vector3d vtMove = ptLe - ptLs ; double dLen = vtMove.LenXY() ;
|
|
|
|
// Sistema di riferimento
|
|
Vector3d vtV1 = - vtToolDir ;
|
|
Vector3d vtV2 = vtMove ; vtV2.Normalize() ;
|
|
|
|
Point3d ptIC = ptLs ; double dZ = ptLs.z ;
|
|
|
|
// Ciclo sui punti
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
double dY = ( j + 0.5) * m_dStep ; double dX = ( i + 0.5) * m_dStep ;
|
|
|
|
Point3d ptCC( dX, dY, 0) ; Vector3d vtCC = ptCC - ptIC ;
|
|
|
|
double dPCL = vtCC * vtV1 ; double dPCT = vtCC * vtV2 ;
|
|
|
|
// Parte cilindrica
|
|
if( dPCL > 0 && dPCL < dCylH) {
|
|
|
|
if ( dPCT > - m_dRadius && dPCT < 0) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dPCT * dPCT) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dPCT >= 0 && dPCT < dLen) {
|
|
|
|
dMin = dZ - m_dRadius ; dMax = dZ + m_dRadius ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dPCT >= dLen && dPCT < dLen + m_dRadius) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - ( dPCT - dLen) * ( dPCT - dLen)) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
// Parte non cilindrica
|
|
else if ( dPCL >= dCylH && dPCL < m_dHeight) {
|
|
|
|
double dPNCL = dPCL - dCylH ; double dPNCT = dPCT ;
|
|
|
|
if ( dPNCT > - m_dRadius - dPNCL * ( m_dTipRadius - m_dRadius) / m_dTipHeight && dPNCT < 0) {
|
|
|
|
double dr = m_dRadius + dPNCL * ( m_dTipRadius - m_dRadius) / m_dTipHeight ;
|
|
|
|
double dH = sqrt( dr * dr - dPNCT * dPNCT) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dPNCT >= 0 && dPNCT < dLen) {
|
|
|
|
double dH = m_dRadius + dPNCL * ( m_dTipRadius - m_dRadius) / m_dTipHeight ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dPNCT >= dLen && dPNCT < dLen + m_dRadius + dPNCL * ( m_dTipRadius - m_dRadius) / m_dTipHeight) {
|
|
|
|
double dr = m_dRadius + dPNCL * ( m_dTipRadius - m_dRadius) / m_dTipHeight ;
|
|
|
|
double dH = sqrt( dr * dr - ( dPNCT - dLen) * ( dPNCT - dLen)) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingXYPlaneGen( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
if ( m_nToolType == CylindricalMill ||
|
|
m_nToolType == BallEndMill)
|
|
return PlaneGenCylBall( ptLs, ptLe, vtToolDir) ;
|
|
else
|
|
return ConusPlaneGen( ptLs, ptLe, vtToolDir) ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::PlaneGenCylBall( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
double dMinZ = min( ptLs.z, ptLe.z) - m_dRadius ;
|
|
double dMaxZ = max( ptLs.z, ptLe.z) + m_dRadius ;
|
|
|
|
// Prima verifica sull'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return true ;
|
|
|
|
Point3d ptI = ptLs ; Point3d ptF = ptLe ;
|
|
|
|
// Quote dei punti ptI e ptF
|
|
double dZ = ptI.z ;
|
|
|
|
Point3d ptIT = ptI - vtToolDir * m_dHeight ;
|
|
Point3d ptFT = ptF - vtToolDir * m_dHeight ;
|
|
|
|
// Bounding box
|
|
double dMinX = min( min( ptI.x, ptIT.x), min( ptF.x, ptFT.x)) - m_dRadius ;
|
|
double dMaxX = max( max( ptI.x, ptIT.x), max( ptF.x, ptFT.x)) + m_dRadius ;
|
|
double dMinY = min( min( ptI.y, ptIT.y), min( ptF.y, ptFT.y)) - m_dRadius ;
|
|
double dMaxY = max( max( ptI.y, ptIT.y), max( ptF.y, ptFT.y)) + m_dRadius ;
|
|
|
|
// Seconda verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
|
|
Vector3d vtMove = ptF - ptI ;
|
|
|
|
if ( vtMove * vtToolDir > 0) {
|
|
Point3d ptTemp = ptI ;
|
|
ptI = ptF ;
|
|
ptF = ptTemp ;
|
|
vtMove = - vtMove ;
|
|
}
|
|
|
|
Vector3d vtMoveOrt = vtMove - ( vtMove * vtToolDir) * vtToolDir ; double dLen2 = vtMoveOrt.Len() ;
|
|
Vector3d vtMoveLong = ( vtMove * vtToolDir) * vtToolDir ; double dLen1 = vtMoveLong.Len() ;
|
|
|
|
// Determino sistema di riferimento del movimento: costruisco un sistrema di vettori ortonormali e destrorsi spiccati dal punto iniziale del movimento
|
|
Vector3d vtV1 = vtToolDir ;
|
|
Vector3d vtV2 = vtMoveOrt ; vtV2.Normalize() ;
|
|
|
|
// Punti iniziale e finale proiettati sul piano
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ; Point3d ptFxy( ptF.x, ptF.y, 0) ;
|
|
|
|
// Determinazione limiti sugli indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
// Ciclo
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
if ( m_nToolType == 1) {
|
|
|
|
Point3d ptC( ( i + 0.5) * m_dStep, ( j + 0.5) * m_dStep, 0) ;
|
|
|
|
Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dProj1 = vtC * vtV1 ;
|
|
double dProj2 = vtC * vtV2 ;
|
|
|
|
GetMMPlaneGenCyl( i, j, dZ, dLen1, dLen2, dProj1, dProj2) ;
|
|
}
|
|
|
|
else if ( m_nToolType == 2)
|
|
GetMMPlaneGenBall( i, j, dZ, dLen1, dLen2, ptIxy, vtMove, vtV1, vtV2) ;
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
inline bool
|
|
VolZmap::GetMinMaxXY( unsigned int nI, unsigned int nJ, double dProj, double dZheight,
|
|
double dSqD, double dPathPerp, double dPathPar, double dScProd)
|
|
{
|
|
// Definisco la variabile altezza della parte cilindrica dell'utensile
|
|
double dCylH ;
|
|
// Definisco variabili per determinazione intervallo da sottrarre
|
|
double dL, dR, dH ;
|
|
|
|
// Caso di utensile generico ( per ora non gestito)
|
|
if ( m_nToolType == 0)
|
|
return false ;
|
|
// Se utensile standard setto altezza della parte cilindrica
|
|
else if ( m_nToolType == 1)
|
|
dCylH = m_dHeight ;
|
|
else if ( m_nToolType == 2)
|
|
dCylH = m_dHeight - m_dRadius ;
|
|
else if ( m_nToolType == 3)
|
|
dCylH = m_dHeight - m_dRCorner ;
|
|
// Caso di utensile non definito
|
|
else
|
|
return false ;
|
|
|
|
// Parametri per isolare la lavorazione della parte cilindrica comune a tutti gli utensili standard
|
|
// Limite nella direziona parallela all'asse dell'utensile e parametro perpendicolare
|
|
double dLimPar, dParPerp ;
|
|
|
|
// Tagli perpendicolari all'asse dell'utensile
|
|
if ( abs( dScProd) < EPS_SMALL) {
|
|
dLimPar = dCylH ;
|
|
dParPerp = dPathPerp ;
|
|
}
|
|
// Tagli paralleli all'asse dell'utensile
|
|
else {
|
|
dLimPar = dPathPar ;
|
|
dParPerp = 0 ;
|
|
}
|
|
|
|
// Parte cilindrica della lavorazione
|
|
if ( dSqD < dLimPar * dLimPar) {
|
|
// Qui il raggio del semicerchio è m_dRadius
|
|
if ( dProj < 0)
|
|
// dH = sqrt( m_dRadius^2 - dProj^2)
|
|
dH = sqrt( m_dRadius * m_dRadius - ( dProj * dProj)) ;
|
|
// Qui l'altezza è costante
|
|
else if ( dProj < dParPerp)
|
|
// dH = m_dRadius
|
|
dH = m_dRadius ;
|
|
// Qui il raggio del semicerchio è m_dRadius
|
|
else if ( dProj < dParPerp + m_dRadius)
|
|
// dH = sqrt( m_dRadius^2 - ( dProj - dParPerp)^2)
|
|
dH = sqrt( m_dRadius * m_dRadius - ( dProj - dParPerp) * ( dProj - dParPerp)) ;
|
|
// Eseguo il taglio
|
|
return SubtractIntervals( nI, nJ, dZheight - dH, dZheight + dH) ;
|
|
}
|
|
// Parte non cilindrica, questa parte esiste solo nei tagli (dScProd = 0) e nel foro con la punta quindi (dScProd < 0)
|
|
// e solo con frese non cilindriche
|
|
else {
|
|
if ( dScProd < EPS_SMALL) {
|
|
// Definisco variabili
|
|
// Caso fresa ball-end
|
|
if ( m_nToolType == 2) {
|
|
// Il raggio è sqrt( m_dRadius^2 - ( sqrt( dSqD) - dLimPar)^2)
|
|
if ( dProj < 0) {
|
|
dL = sqrt( dSqD) - dLimPar ;
|
|
dR = sqrt( m_dRadius * m_dRadius - dL * dL) ;
|
|
dH = sqrt( dR * dR - (dProj * dProj)) ;
|
|
}
|
|
// Qui dH non dipende da dProj
|
|
else if ( dProj < dParPerp) {
|
|
dL = sqrt( dSqD) - dLimPar ;
|
|
dH = sqrt( m_dRadius * m_dRadius - dL * dL) ;
|
|
}
|
|
// E' analogo al primo caso con la sostituzione di dProj con dProj - dParPerp
|
|
else if ( dProj < dParPerp + m_dRadius) { // In questo caso è equivalente a else
|
|
dL = sqrt( dSqD) - dLimPar ;
|
|
dR = sqrt( m_dRadius * m_dRadius - dL * dL) ;
|
|
dH = sqrt( dR * dR - ((dProj - dParPerp) * (dProj - dParPerp))) ;
|
|
}
|
|
}
|
|
// Caso di fresa bull-nose
|
|
else if ( m_nToolType == 3) {
|
|
// Raggio semicerchio m_dRadius - m_dRCorner + sqrt( m_dRCorner^2 - ( sqrt( dSqD) - dLimPar)^2)
|
|
if ( dProj < 0) {
|
|
dL = sqrt( dSqD) - dLimPar ;
|
|
dR = m_dRadius - m_dRCorner + sqrt( m_dRCorner * m_dRCorner - dL * dL) ;
|
|
dH = sqrt( dR * dR - ( dProj * dProj)) ;
|
|
}
|
|
// Qui dH non dipende da dProj
|
|
else if ( dProj < dParPerp) {
|
|
dL = sqrt( dSqD) - dLimPar ;
|
|
dH = m_dRadius - m_dRCorner + sqrt( m_dRCorner * m_dRCorner - dL * dL) ;
|
|
}
|
|
// E' analogo al primo caso con la sostituzione di di dProj con dProj - dParPerp
|
|
else if ( dProj < dParPerp + m_dRadius) { // In questo caso equivalente a else
|
|
dL = sqrt( dSqD) - dLimPar ;
|
|
dR = m_dRadius - m_dRCorner + sqrt( m_dRCorner * m_dRCorner - dL * dL) ;
|
|
dH = sqrt( dR * dR - ( ( dProj - dParPerp) * ( dProj - dParPerp))) ;
|
|
}
|
|
}
|
|
return SubtractIntervals( nI, nJ, dZheight - dH, dZheight + dH) ;
|
|
}
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
inline bool
|
|
VolZmap::GetMMPlaneGenCyl( unsigned int i, unsigned int j, double dZ,
|
|
double dLen1, double dLen2, double dProj1, double dProj2)
|
|
{
|
|
double dMin, dMax ;
|
|
|
|
if ( dProj2 > - m_dRadius && dProj2 < 0) {
|
|
|
|
if ( dProj1 < 0 && dProj1 > - m_dHeight) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dProj2 * dProj2) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
else if ( dProj1 <= - m_dHeight) {
|
|
|
|
if ( ( dProj2 < dLen2 - m_dRadius && dProj1 > - m_dHeight - ( dLen1 / dLen2) * ( dProj2 + m_dRadius))
|
|
|| ( dProj2 >= dLen2 - m_dRadius && dProj1 > - m_dHeight - dLen1)) { // In questo costrutto if-else non c'è bisogno di specificare nient'altro perché già siamo nella regione - m_dRadius < dProj2 < 0
|
|
|
|
double dPar = m_dHeight + ( dLen1 / dLen2) * ( dProj2 + m_dRadius) + dProj1 ;
|
|
double dL = m_dRadius - ( dLen2 / dLen1) * dPar ;
|
|
double dH = ( m_dRadius * m_dRadius - dL * dL > 0 ? sqrt( m_dRadius * m_dRadius - dL * dL) : 0) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
else if ( dProj2 >= 0 && dProj2 < m_dRadius) {
|
|
|
|
if ( dProj1 < 0 && ( ( dProj2 < dLen2 && dProj1 > - ( dLen1 / dLen2) * dProj2) || ( dProj2 >= dLen2 && dProj1 > - dLen1))) {
|
|
|
|
double dPar = dProj2 + ( dLen2 / dLen1) * dProj1 ;
|
|
/* Oppure
|
|
double dPar1 = ( dLen1 / dLen2) * dProj2 + dProj1 ;
|
|
double dPar2 = ( dLen2 / dLen1) * dPar1 ; */
|
|
double dH = ( m_dRadius * m_dRadius - dPar * dPar > 0 ? sqrt( m_dRadius * m_dRadius - dPar * dPar) : 0) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj2 < dLen2 && dProj1 <= - ( dLen1 / dLen2) * dProj2) {
|
|
|
|
if ( dProj1 > - ( dLen1 / dLen2) * dProj2 - m_dHeight) {
|
|
|
|
dMin = dZ - m_dRadius ; dMax = dZ + m_dRadius ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj1 <= - ( dLen1 / dLen2) * dProj2 - m_dHeight) {
|
|
|
|
if ( ( dProj2 < dLen2 - m_dRadius && dProj1 > - ( dLen1 / dLen2) * ( dProj2 + m_dRadius) - m_dHeight)
|
|
|| ( dProj2 >= dLen2 - m_dRadius && dProj1 > - m_dHeight - dLen1)) { // modificato qui
|
|
|
|
double dPar = m_dHeight + ( dLen1 / dLen2) * ( dProj2 + m_dRadius) + dProj1 ;
|
|
double dL = m_dRadius - ( dLen2 / dLen1) * dPar ;
|
|
double dH = ( m_dRadius * m_dRadius - dL * dL > 0 ? sqrt( m_dRadius * m_dRadius - dL * dL) : 0) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
else if ( dProj2 >= dLen2 && dProj1 < - dLen1) {
|
|
|
|
if ( dProj1 > - m_dHeight - dLen1) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - ( dProj2 - dLen2) * ( dProj2 - dLen2)) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
else if ( dProj2 >= m_dRadius && dProj2 < dLen2) {
|
|
|
|
if ( dProj1 < - ( dLen1 / dLen2) * ( dProj2 - m_dRadius) && dProj1 > - ( dLen1 / dLen2) * dProj2) {
|
|
|
|
double dL = ( dLen2 / dLen1) * ( ( dLen1 / dLen2) * dProj2 + dProj1) ;
|
|
double dH = ( m_dRadius * m_dRadius - dL * dL > 0 ? sqrt( m_dRadius * m_dRadius - dL * dL) : 0) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj1 <= - ( dLen1 / dLen2) * dProj2 && dProj1 > - ( dLen1 / dLen2) * dProj2 - m_dHeight) {
|
|
|
|
dMin = dZ - m_dRadius ; dMax = dZ + m_dRadius ;
|
|
|
|
SubtractIntervals( i, j, dMin , dMax) ;
|
|
}
|
|
else if ( dProj1 <= - ( dLen1 / dLen2) * dProj2 - m_dHeight) {
|
|
|
|
if ( ( dProj2 < dLen2 - m_dRadius && dProj1 > - ( dLen1 / dLen2) * ( dProj2 + m_dRadius) - m_dHeight)
|
|
|| ( dProj2 >= dLen2 - m_dRadius && dProj1 > - m_dHeight - dLen1)) {
|
|
|
|
double dPar = m_dHeight + ( dLen1 / dLen2) * ( dProj2 + m_dRadius) + dProj1 ;
|
|
double dL = m_dRadius - ( dLen2 / dLen1) * dPar ;
|
|
double dH = ( m_dRadius * m_dRadius - dL * dL > 0 ? sqrt( m_dRadius * m_dRadius - dL * dL) : 0) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
else if ( dProj2 >= dLen2 && dProj2 < dLen2 + m_dRadius) {
|
|
|
|
if ( dProj1 < - ( dLen1 / dLen2) * ( dProj2 - m_dRadius) && dProj1 >= - dLen1) {
|
|
|
|
double dL = ( dLen2 / dLen1) * ( ( dLen1 / dLen2) * dProj2 + dProj1) ;
|
|
double dH = ( m_dRadius * m_dRadius - dL * dL > 0 ? sqrt( m_dRadius * m_dRadius - dL * dL) : 0) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj1 < - dLen1 && dProj1 > - dLen1 - m_dHeight) {
|
|
|
|
double dL = dProj2 - dLen2 ;
|
|
double dH = sqrt( m_dRadius * m_dRadius - dL * dL) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
inline bool
|
|
VolZmap::GetMMPlaneGenBall( unsigned int i, unsigned int j, double dZ, double dLen1, double dLen2,
|
|
Point3d ptIxy, Vector3d vtMove, Vector3d vtV1, Vector3d vtV2)
|
|
{
|
|
double dMin, dMax ;
|
|
|
|
Point3d ptC( ( i + 0.5) * m_dStep, ( j + 0.5) * m_dStep, 0) ;
|
|
|
|
Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dProj1 = vtC * vtV1 ; // vtV1, vtV2 sono paralleli al piano
|
|
double dProj2 = vtC * vtV2 ;
|
|
|
|
double dCH = m_dHeight - m_dRadius ;
|
|
double dLMove = vtMove.LenXY() ;
|
|
|
|
Point3d ptCS = ptIxy - dCH * vtV1 ; Point3d ptCE = ptCS + vtMove ; // vtMove è orizzontale
|
|
|
|
Vector3d vtCS = ptC - ptCS ; Vector3d vtCE = ptC - ptCE ;
|
|
|
|
double dProjMove = ( vtCS * vtMove) / dLMove ;
|
|
|
|
Vector3d vtCSP = vtCS - ( ( vtCS * vtMove) / ( dLMove * dLMove)) * vtMove ;
|
|
|
|
double dSQDist = vtCSP.SqLenXY() ;
|
|
double dSQDistS = vtCS.SqLenXY() ;
|
|
double dSQDistE = vtCE.SqLenXY() ;
|
|
|
|
// parte cilindrica
|
|
|
|
if ( dProj2 > - m_dRadius && dProj2 < 0) {
|
|
|
|
if ( dProj1 > - dCH && dProj1 < 0) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dProj2 * dProj2) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( dProj2 >= 0 && dProj2 < m_dRadius) {
|
|
|
|
if ( dProj1 < 0 && ( ( dProj2 < dLen2 && dProj1 > - ( dLen1 / dLen2) * dProj2) || ( dProj2 >= dLen2 && dProj1 > - dLen1))) {
|
|
|
|
double dPar = dProj2 + ( dLen2 / dLen1) * dProj1 ;
|
|
/* Oppure
|
|
double dPar1 = ( dLen1 / dLen2) * dProj2 + dProj1 ;
|
|
double dPar2 = ( dLen2 / dLen1) * dPar1 ; */
|
|
double dH = ( m_dRadius * m_dRadius - dPar * dPar > 0 ? sqrt( m_dRadius * m_dRadius - dPar * dPar) : 0) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj2 < dLen2 && dProj1 <= - ( dLen1 / dLen2) * dProj2 && dProj1 > - ( dLen1 / dLen2) * dProj2 - dCH) {
|
|
|
|
dMin = dZ - m_dRadius ; dMax = dZ + m_dRadius ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj2 >= dLen2 && dProj1 <= - dLen1 && dProj1 > - dLen1 - dCH) {
|
|
|
|
double dL = dProj2 - dLen2 ;
|
|
double dH = sqrt( m_dRadius * m_dRadius - dL * dL) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( dProj2 >= m_dRadius && dProj1 < - ( dLen1 / dLen2) * ( dProj2 - m_dRadius)) {
|
|
|
|
if ( dProj2 < dLen2 && dProj1 > - ( dLen1 / dLen2) * dProj2) {
|
|
|
|
double dPar = dProj2 + ( dLen2 / dLen1) * dProj1 ;
|
|
double dH = ( m_dRadius * m_dRadius - dPar * dPar > 0 ? sqrt( m_dRadius * m_dRadius - dPar * dPar) : 0) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj2 < dLen2 && dProj1 <= - ( dLen1 / dLen2) * dProj2 && dProj1 > - ( dLen1 / dLen2) * dProj2 - dCH) {
|
|
|
|
dMin = dZ - m_dRadius ; dMax = dZ + m_dRadius ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj2 >= dLen2 && dProj2 < dLen2 + m_dRadius) {
|
|
|
|
if ( dProj1 > - dLen1) {
|
|
|
|
double dPar = dProj2 + ( dLen2 / dLen1) * dProj1 ;
|
|
double dH = ( m_dRadius * m_dRadius - dPar * dPar > 0 ? sqrt( m_dRadius * m_dRadius - dPar * dPar) : 0) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj1 <= - dLen1 && dProj1 > - dLen1 - dCH) {
|
|
|
|
double dL = dProj2 - dLen2 ;
|
|
double dH = sqrt( m_dRadius * m_dRadius - dL * dL) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
// parte non cilindrica
|
|
if ( dProjMove > - m_dRadius && dProjMove < 0) {
|
|
|
|
if ( dSQDistS < m_dRadius * m_dRadius) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSQDistS) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( dProjMove >= 0 && dProjMove < dLMove) {
|
|
|
|
if ( dSQDist < m_dRadius * m_dRadius) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSQDist) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else {
|
|
|
|
if ( dSQDistE < m_dRadius * m_dRadius) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSQDistE) ;
|
|
|
|
dMin = dZ - dH ; dMax = dZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::ConusPlaneGen( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
double dMin, dMax ;
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BoundingBox( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ) ;
|
|
|
|
if ( Control == false)
|
|
return true ;
|
|
|
|
double dMinRad = min( m_dRadius, m_dTipRadius) ;
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
double dZH = ptLs.z ;
|
|
|
|
m_nToolType = CylindricalMill ;
|
|
m_dHeight = dCylH ;
|
|
|
|
MillingXYPlaneGen( ptLs, ptLe, vtToolDir) ;
|
|
|
|
m_nToolType = ConusMill ;
|
|
m_dHeight = m_dHeight + m_dTipHeight ;
|
|
|
|
Point3d ptI, ptF ;
|
|
|
|
Vector3d vtV1 ;
|
|
|
|
if ( m_dTipRadius < m_dRadius) {
|
|
|
|
vtV1 = vtToolDir ;
|
|
ptI = ( vtV1 * ( ptLe - ptLs) > 0 ? ptLs - vtV1 * dCylH : ptLe - vtV1 * dCylH) ;
|
|
ptF = ( vtV1 * ( ptLe - ptLs) > 0 ? ptLe - vtV1 * dCylH : ptLs - vtV1 * dCylH) ;
|
|
}
|
|
|
|
else {
|
|
vtV1 = - vtToolDir ;
|
|
ptI = ( vtToolDir * ( ptLe - ptLs) > 0 ? ptLe + vtV1 * m_dHeight : ptLs + vtV1 * m_dHeight) ;
|
|
ptF = ( vtToolDir * ( ptLe - ptLs) > 0 ? ptLs + vtV1 * m_dHeight : ptLe + vtV1 * m_dHeight) ;
|
|
}
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
|
|
Vector3d vtMove = ptF - ptI ;
|
|
Vector3d vtLong = ( vtMove * vtV1) * vtV1 ; double dLen1 = vtLong.LenXY() ;
|
|
Vector3d vtOrt = vtMove - vtLong ; double dLen2 = vtOrt.LenXY() ;
|
|
Vector3d vtTemp = vtV1 ; vtTemp.Rotate( Z_AX, 90) ;
|
|
|
|
vtMove.Normalize() ;
|
|
|
|
Vector3d vtV2 = ( vtMove * vtTemp > 0 ? vtTemp : - vtTemp) ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
|
|
double dTan = ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dRatio = ( vtMove * vtV1) / ( vtMove * vtV2) ;
|
|
|
|
double dCos = dTan * dRatio ;
|
|
double dSin = ( abs( dCos) < 1 ? sqrt( 1 - dCos * dCos) : 0) ;
|
|
|
|
double dDen = sqrt( 1 + dTan * dTan) ;
|
|
|
|
double dDeltaR = dMaxRad - dMinRad ;
|
|
double dMinLim = dMinRad * dCos ;
|
|
double dMaxLim = dMaxRad * dCos ;
|
|
|
|
|
|
// Versori normali e prodotti scalari per determinare i piani
|
|
Vector3d vtNInf = - ( dTan / dDen) * vtV1 + ( dCos / dDen) * vtV2 + ( dSin / dDen) * vtV3 ;
|
|
Vector3d vtNSup = - ( dTan / dDen) * vtV1 + ( dCos / dDen) * vtV2 - ( dSin / dDen) * vtV3 ;
|
|
|
|
Point3d ptV = ptI - vtV1 * ( dMaxRad * m_dTipHeight) / ( dMaxRad - dMinRad) ;
|
|
Vector3d vtR0 = ptV - ORIG ;
|
|
double dDotInf = vtR0 * vtNInf ;
|
|
double dDotSup = vtR0 * vtNSup ;
|
|
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
Point3d ptC( ( i + 0.5) * m_dStep, ( j + 0.5) * m_dStep, 0) ;
|
|
|
|
|
|
Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dProj1 = vtC * vtV1 ; double dProj2 = vtC * vtV2 ;
|
|
|
|
if ( dRatio <= m_dTipHeight / dDeltaR) {
|
|
|
|
if ( dProj1 > - m_dTipHeight && dProj1 < 0 &&
|
|
dProj2 > - dMaxRad - dTan * dProj1 &&
|
|
dProj2 < dMaxLim + dProj1 * ( dMaxLim - dMinLim) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad + dTan * dProj1 ;
|
|
double dH = sqrt( dr * dr - dProj2 * dProj2) ;
|
|
|
|
dMin = dZH - dH ; dMax = dZH + dH ;
|
|
|
|
SubtractIntervals( i, j , dMin, dMax) ;
|
|
}
|
|
else if ( dProj1 > dLen1 - m_dTipHeight && dProj1 < dLen1 &&
|
|
dProj2 > dLen2 - dMaxRad - dTan * dProj1 &&
|
|
dProj2 < dLen2 + dMaxLim + dProj1 * ( dMaxLim - dMinLim) / m_dTipHeight) { // Se due sistemi di riferimento hanno stessi versori di base e differiscono semplicemente per le origini,
|
|
// le proiezioni di un vettore sugli assi nei due sistemi differiscono per le componenti del vettore che congiunge le origini.
|
|
double dr = dMaxRad + dTan * ( dProj1 - dLen1) ;
|
|
double dH = sqrt( dr * dr - ( dProj2 - dLen2) * ( dProj2 - dLen2)) ;
|
|
|
|
dMin = dZH - dH ; dMax = dZH + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj1 >= 0 && dProj1 < dLen1 &&
|
|
dProj2 > - dMaxRad + dProj1 * dLen2 / dLen1 &&
|
|
dProj2 < dMaxLim + dProj1 * dLen2 / dLen1) {
|
|
|
|
double dr = abs( dProj2 - dProj1 * dLen2 / dLen1) ; // Proj2 del punto meno Proj2 del centro del cerchio
|
|
double dH = sqrt( dMaxRad * dMaxRad - dr * dr) ;
|
|
|
|
dMin = dZH - dH ; dMax = dZH + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj1 > - m_dTipHeight && dProj1 < - m_dTipHeight + dLen1 && // Idem con patate
|
|
dProj2 > dMinLim + dProj1 * ( dLen2 / dLen1) &&
|
|
dProj2 < dMinRad + dProj1 * ( dLen2 / dLen1)) {
|
|
|
|
double dr = dProj2 - dProj1 * dLen2 / dLen1 ;
|
|
double dH = sqrt( dMinRad * dMinRad - dr * dr) ;
|
|
|
|
dMin = dZH - dH ; dMax = dZH + dH ;
|
|
|
|
SubtractIntervals( i, j , dMin, dMax) ;
|
|
}
|
|
else { // L'unico dominio non normale lo detrerminiamo per sottrazione :)
|
|
|
|
dMin = ( dDotInf - ptC.x * vtNInf.x - ptC.y * vtNInf.y) / vtNInf.z ;
|
|
dMax = ( dDotSup - ptC.x * vtNSup.x - ptC.y * vtNSup.y) / vtNSup.z ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else {
|
|
|
|
if ( dProj1 > - m_dTipHeight && dProj1 <= 0 &&
|
|
dProj2 > - dMaxRad - dProj1 * ( dMaxRad - dMinRad) / m_dTipHeight &&
|
|
dProj2 < dMaxRad + dProj1 * ( dMaxRad - dMinRad) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad + dProj1 * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - dProj2 * dProj2) ;
|
|
|
|
dMin = dZH - dH ; dMax = dZH + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dProj1 > 0 && dProj1 < dLen1 &&
|
|
dProj2 > - dMaxRad + dProj1 * ( dLen2 / dLen1) &&
|
|
dProj2 < dMaxRad + dProj1 * ( dLen2 / dLen1)) {
|
|
|
|
double dr = abs( dProj2 - dProj1 * ( dLen2 / dLen1)) ;
|
|
double dH = sqrt( dMaxRad * dMaxRad - dr * dr) ;
|
|
|
|
dMin = dZH - dH ; dMax = dZH + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
|
|
// DeltaZ != 0
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingXYVert( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
double dMin, dMax ;
|
|
|
|
// Determinazione dell'interferenza dell'utensile con lo Zmap, bouding box e limiti su indici
|
|
bool Control = BoundingBox( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
double dZDown = min( ptLs.z, ptLe.z) ;
|
|
double dZUp = max( ptLs.z, ptLe.z) ;
|
|
|
|
// Definizione di un sistema di riferimento
|
|
Vector3d vtV1 = - vtToolDir ;
|
|
Vector3d vtV2 = vtV1 ; vtV2.Rotate(Z_AX, 90) ;
|
|
|
|
Point3d ptIC = ptLs ; Point3d ptINC = ptLs + dCylH * vtV1 ;
|
|
|
|
Point3d ptICxy( ptIC.x, ptIC.y, 0) ; Point3d ptINCxy( ptINC.x, ptINC.y, 0) ;
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep , dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtCC = ptC - ptICxy ; Vector3d vtNCC = ptC - ptINCxy ;
|
|
|
|
double dCPL = vtCC * vtV1 ; double dCPT = vtCC * vtV2 ;
|
|
|
|
double dNCPL = vtNCC * vtV1 ; double dNCPT = vtNCC * vtV2 ;
|
|
|
|
// Parte cilindrica
|
|
if ( dCPL > 0 && dCPL < dCylH && dCPT > - m_dRadius && dCPT < m_dRadius) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dCPT * dCPT) ;
|
|
|
|
dMin = dZDown - dH ; dMax = dZUp + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
// Parte non cilindrica
|
|
if ( m_nToolType == BallEndMill) {
|
|
|
|
double dSqLen = vtNCC.SqLenXY() ;
|
|
|
|
if ( dNCPL >= 0 && dSqLen < m_dRadius * m_dRadius) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSqLen) ;
|
|
|
|
dMin = dZDown - dH ; dMax = dZUp + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( m_nToolType == BullNoseMill) {
|
|
|
|
if ( dNCPL >= 0 && dNCPL < m_dTipHeight) {
|
|
|
|
double dR = m_dTipRadius + sqrt( m_dRCorner * m_dRCorner - dNCPL * dNCPL) ;
|
|
|
|
if ( dNCPT > - dR && dNCPT < dR) {
|
|
|
|
double dH = sqrt( dR * dR - dNCPT * dNCPT) ;
|
|
|
|
dMin = dZDown - dH ; dMax = dZUp + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
else if ( m_nToolType == ConusMill) {
|
|
|
|
if ( dNCPL >= 0 && dNCPL < m_dTipHeight &&
|
|
dNCPT > - m_dRadius - dNCPL * ( m_dTipRadius - m_dRadius) / m_dTipHeight &&
|
|
dNCPT < m_dRadius + dNCPL * ( m_dTipRadius - m_dRadius) / m_dTipHeight) {
|
|
|
|
double dr = m_dRadius + dNCPL * ( m_dTipRadius - m_dRadius) / m_dTipHeight ;
|
|
|
|
double dH = sqrt( dr * dr - dNCPT * dNCPT) ;
|
|
|
|
dMin = dZDown - dH ; dMax = dZUp + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingXYLongVert( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
if ( m_nToolType == CylindricalMill ||
|
|
m_nToolType == BallEndMill)
|
|
|
|
return XYLongVertCylBall( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else if ( m_nToolType == ConusMill)
|
|
|
|
return XYLongVertConus( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else
|
|
// Casi al momento non gestiti
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::XYLongVertCylBall( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
unsigned int nStartI, nEndI, nStartJ, nEndJ ;
|
|
double dMin, dMax ;
|
|
|
|
bool Control = BoundingBox( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ) ;
|
|
|
|
// Se Control è falso non vi è interferenza fra utensile e Zmap
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
double dDeltaZ = ptLe.z - ptLs.z;
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
|
|
Point3d ptI, ptF, ptBI, ptBF ;
|
|
Vector3d vtV1, vtV2 ;
|
|
Vector3d vtMove = ptLe - ptLs ;
|
|
|
|
// Studio della parte sferica
|
|
if ( ptLs.z < ptLe.z) {
|
|
|
|
ptBI = ptLs - vtToolDir * dCylH ;
|
|
ptBF = ptLe - vtToolDir * dCylH ;
|
|
}
|
|
|
|
else {
|
|
ptBI = ptLe - vtToolDir * dCylH ;
|
|
ptBF = ptLs - vtToolDir * dCylH ;
|
|
}
|
|
|
|
Point3d ptBIxy( ptBI.x, ptBI.y, 0) ;
|
|
Vector3d vtBMove = ptBF - ptBI ; vtBMove.Normalize() ;
|
|
Vector3d vtBV1 = ( vtToolDir * vtBMove > 0 ? vtToolDir : - vtToolDir) ;
|
|
|
|
double dDeltaBZ = ptBF.z - ptBI.z ;
|
|
double dOriz = vtBMove * Z_AX ;
|
|
double dVert = vtBMove * vtBV1 ;
|
|
|
|
double dSemiAxMin = m_dRadius * dOriz ;
|
|
|
|
// Studio delle simmetrie della parte cilindrica
|
|
if ( vtToolDir * vtMove < 0 && dDeltaZ < 0) {
|
|
|
|
ptI = ptLe - vtToolDir * dCylH ;
|
|
ptF = ptLs - vtToolDir * dCylH ;
|
|
vtMove = - vtMove ;
|
|
vtV1 = - vtToolDir ;
|
|
dDeltaZ = - dDeltaZ ;
|
|
}
|
|
|
|
else if ( vtToolDir * vtMove > 0 && dDeltaZ > 0) {
|
|
ptI = ptLs - vtToolDir * dCylH ;
|
|
ptF = ptLe - vtToolDir * dCylH ;
|
|
vtV1 = - vtToolDir ;
|
|
}
|
|
|
|
else if ( vtToolDir * vtMove > 0 && dDeltaZ < 0) {
|
|
ptI = ptLe ;
|
|
ptF = ptLs ;
|
|
vtV1 = vtToolDir ;
|
|
vtMove = - vtMove ;
|
|
dDeltaZ = - dDeltaZ ;
|
|
}
|
|
|
|
else {
|
|
ptI = ptLs ;
|
|
ptF = ptLe ;
|
|
vtV1 = vtToolDir ;
|
|
}
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ; double dZI = ptI.z ;
|
|
|
|
vtV2 = vtV1 ; vtV2.Rotate( Z_AX, 90) ;
|
|
|
|
double dLen = abs( vtMove * vtV1) ;
|
|
|
|
// Ciclo
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dProj1 = - vtC * vtV1 ; double dProj2 = vtC * vtV2 ;
|
|
|
|
Vector3d vtBC = ptC - ptBIxy ;
|
|
|
|
double dBpr1 = vtBC * vtBV1 ; double dBpr2 = vtBC * vtV2 ;
|
|
|
|
// Parte cilindrica
|
|
|
|
if ( dProj2 > - m_dRadius && dProj2 < m_dRadius &&
|
|
dProj1 > 0 && dProj1 < dLen + dCylH) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dProj2 * dProj2) ;
|
|
|
|
// Massimi
|
|
if ( dProj1 > 0 && dProj1 < dLen) {
|
|
|
|
double dZ0 = ptI.z + dProj1 * ( dDeltaZ / dLen) ;
|
|
|
|
dMax = dZ0 + dH ;
|
|
}
|
|
else if ( dProj1 >= dLen && dProj1 < dLen + dCylH)
|
|
|
|
dMax = ptI.z + dDeltaZ + dH ;
|
|
|
|
// Minimi
|
|
if ( dProj1 > 0 && dProj1 < dCylH)
|
|
|
|
dMin = ptI.z - dH ;
|
|
|
|
else if ( dProj1 >= dCylH && dProj1 < dLen + dCylH) {
|
|
|
|
double dZ0 = ptI.z + ( dProj1 - dCylH) * ( dDeltaZ / dLen) ;
|
|
|
|
dMin = dZ0 - dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
// Parte Non cilindrica
|
|
if ( m_nToolType == BallEndMill) {
|
|
|
|
double dSqDistO = dBpr2 * dBpr2 ;
|
|
double dSqDistI = vtBC.SqLenXY() ;
|
|
double dSqDistF = ( dBpr1 - dLen) * ( dBpr1 - dLen) + dBpr2 * dBpr2 ;
|
|
|
|
if ( ( dBpr1 < 0 && dSqDistI < m_dRadius * m_dRadius) ||
|
|
( dBpr1 >= 0 && dBpr1 < dLen && dSqDistO < m_dRadius * m_dRadius) ||
|
|
( dBpr1 >= dLen && dSqDistF < m_dRadius * m_dRadius)) {
|
|
|
|
// Massimi
|
|
if ( dBpr1 < - dSemiAxMin * sqrt( 1 - dSqDistO / ( m_dRadius * m_dRadius))) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSqDistI) ;
|
|
|
|
dMax = ptBI.z + dH ;
|
|
}
|
|
else if ( dBpr1 >= - dSemiAxMin * sqrt( 1 - dSqDistO / ( m_dRadius * m_dRadius)) &&
|
|
dBpr1 < dLen - dSemiAxMin * sqrt( 1 - dSqDistO / ( m_dRadius * m_dRadius))) {
|
|
|
|
double dZ0 = ptBI.z + dVert * sqrt( m_dRadius * m_dRadius - dSqDistO) ;
|
|
|
|
dMax = dZ0 + dDeltaBZ * ( dBpr1 + dSemiAxMin * sqrt( 1 - dSqDistO / ( m_dRadius * m_dRadius))) / dLen ;
|
|
}
|
|
else {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSqDistF) ;
|
|
|
|
dMax = ptBF.z + dH ;
|
|
}
|
|
|
|
// Minimi
|
|
if ( dBpr1 < dSemiAxMin * sqrt( 1 - dSqDistO / ( m_dRadius * m_dRadius))) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSqDistI) ;
|
|
|
|
dMin = ptBI.z - dH ;
|
|
}
|
|
else if ( dBpr1 >= dSemiAxMin * sqrt( 1 - dSqDistO / ( m_dRadius * m_dRadius)) &&
|
|
dBpr1 < dLen + dSemiAxMin * sqrt( 1 - dSqDistO / ( m_dRadius * m_dRadius))) {
|
|
|
|
double dZ0 = ptBI.z - dVert * sqrt( m_dRadius * m_dRadius - dSqDistO) ;
|
|
|
|
dMin = dZ0 + dDeltaBZ * ( dBpr1 - dSemiAxMin * sqrt( 1 - dSqDistO / ( m_dRadius * m_dRadius))) / dLen ;
|
|
}
|
|
else {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSqDistF) ;
|
|
|
|
dMin = ptBF.z - dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
/*
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::XYLongVertConus( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir) {
|
|
|
|
double dMin, dMax ;
|
|
unsigned int nStartI, nEndI, nStartJ, nEndJ ;
|
|
|
|
bool Control = BoundingBox( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ) ;
|
|
|
|
if ( Control == false)
|
|
|
|
return true ;
|
|
|
|
// Parte cilindrica
|
|
m_nToolType = CylindricalMill ;
|
|
|
|
double dSafeHeight = m_dHeight ;
|
|
double dSafeTipHeight = m_dTipHeight ;
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
|
|
m_dHeight = dCylH ;
|
|
m_dTipHeight = 0 ;
|
|
|
|
XYLongVertCylBall( ptLs, ptLe, vtToolDir) ;
|
|
|
|
m_nToolType = ConusMill ;
|
|
m_dHeight = dSafeHeight ;
|
|
m_dTipHeight = dSafeTipHeight ;
|
|
|
|
// Parte conica
|
|
|
|
Point3d ptI, ptF ;
|
|
Vector3d vtV1 ;
|
|
|
|
double dMinRad = min( m_dRadius, m_dTipRadius) ;
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
double dDeltaR = dMaxRad - dMinRad ;
|
|
|
|
double dStem ;
|
|
|
|
if ( m_dRadius > m_dTipRadius) {
|
|
|
|
vtV1 = vtToolDir ;
|
|
dStem = - dCylH ;
|
|
}
|
|
else {
|
|
|
|
vtV1 = - vtToolDir ;
|
|
dStem = m_dHeight ;
|
|
}
|
|
|
|
|
|
if ( vtV1 * ( ptLe - ptLs) * ( ptLe.z - ptLs.z) > 0) {
|
|
|
|
ptI = ( ptLe.z - ptLs.z < 0 ? ptLs + vtV1 * dStem : ptLe + vtV1 * dStem) ;
|
|
ptF = ( ptLe.z - ptLs.z < 0 ? ptLe + vtV1 * dStem : ptLs + vtV1 * dStem) ;
|
|
}
|
|
else {
|
|
|
|
ptI = ( ptLe.z - ptLs.z > 0 ? ptLs + vtV1 * dStem : ptLe + vtV1 * dStem) ;
|
|
ptF = ( ptLe.z - ptLs.z > 0 ? ptLe + vtV1 * dStem : ptLs + vtV1 * dStem) ;
|
|
}
|
|
|
|
double dZI = ptI.z ;
|
|
double dDeltaZ = ptF.z - ptI.z ;
|
|
|
|
Vector3d vtV2 = vtV1 ; vtV2.Rotate(Z_AX, 90) ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
Vector3d vtMove = ptF - ptI ;
|
|
Vector3d vtMoveXY( vtMove.x, vtMove.y, 0) ; double dPLen = vtMoveXY.LenXY() ;
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
|
|
Point3d ptV = ptI - vtV1 * ( ( dMaxRad * m_dTipHeight) / dDeltaR) ;
|
|
|
|
// Apertura del cono e parametri per determinare i piani
|
|
double dTan = ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dRatio = ( vtMove * vtV1) / ( vtMove * vtV3) ;
|
|
double dCos = dTan * dRatio ;
|
|
double dSen = ( 1 - dCos * dCos > 0 ? sqrt( 1 - dCos * dCos) : 0) ;
|
|
|
|
// Versori normali e prodotti scalari per per determinare i piani
|
|
Vector3d vtNs = - ( dTan / sqrt( 1 + dTan * dTan)) * vtV1 + ( dCos / sqrt( 1 + dTan * dTan)) * vtV3 - ( sqrt( 1 - dCos * dCos) / sqrt( 1 + dTan * dTan)) * vtV2 ;
|
|
Vector3d vtNd = - ( dTan / sqrt( 1 + dTan * dTan)) * vtV1 + ( dCos / sqrt( 1 + dTan * dTan)) * vtV3 + ( sqrt( 1 - dCos * dCos) / sqrt( 1 + dTan * dTan)) * vtV2 ;
|
|
Vector3d vtR0 = ptV - ORIG ;
|
|
double dDots = vtR0 * vtNs ;
|
|
double dDotd = vtR0 * vtNd ;
|
|
|
|
|
|
// Ciclo
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i)
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dProj1 = vtC * vtV1 ; double dProj2 = vtC * vtV2 ; double dOrtLen = abs( dProj2) ;
|
|
|
|
if ( ( dProj1 > 0 && dProj1 < dPLen && dOrtLen < dMaxRad) ||
|
|
( dProj1 >= dPLen && dProj1 < dPLen + m_dTipHeight &&
|
|
dOrtLen < dMaxRad + ( dProj1 - dPLen) * ( dDeltaR / m_dTipHeight))) {
|
|
|
|
// DeltaZ < 0
|
|
if ( dDeltaZ < 0) {
|
|
|
|
if ( dRatio <= 1 / dTan) {
|
|
|
|
if ( dProj1 < m_dTipHeight && dOrtLen < dMaxRad * dSen - dProj1 * dDeltaR * dSen / m_dTipHeight) { // Il limite inferiore d dProj1 è già stato imposto dall' if più esterno
|
|
|
|
double dr = dMaxRad - dProj1 * dDeltaR / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - dOrtLen * dOrtLen) ;
|
|
|
|
dMax = dZI + dH ;
|
|
}
|
|
else if ( dProj1 > 0 && dProj1 < dPLen &&
|
|
dOrtLen > dMaxRad * dSen && dOrtLen < dMaxRad) {
|
|
|
|
double dH = sqrt( dMaxRad * dMaxRad - dOrtLen * dOrtLen) ;
|
|
|
|
dMax = dZI + dH + dProj1 * dDeltaZ / dPLen ;
|
|
}
|
|
else if ( dProj1 > m_dHeight && dProj1 < dPLen + m_dHeight &&
|
|
dOrtLen < dMinRad * dSen) {
|
|
|
|
double dH = sqrt( dMinRad * dMinRad - dOrtLen * dOrtLen) ;
|
|
|
|
dMax = dZI + dH + ( dProj1 - m_dTipHeight) * dDeltaZ / dPLen ;
|
|
}
|
|
else if ( dProj1 >= dPLen &&
|
|
dOrtLen > dMaxRad * dSen - ( dProj1 - dPLen) * dDeltaR * dSen / m_dTipHeight) { // I limiti superiori sono già stati imposti dall' if più esterno
|
|
|
|
double dr = dMaxRad - ( dProj1 - dPLen) * dDeltaR / m_dTipHeight ;
|
|
|
|
double dH = sqrt( dr * dr - dOrtLen * dOrtLen) ;
|
|
|
|
dMax = dZI + dDeltaZ + dH ;
|
|
}
|
|
else
|
|
|
|
dMin = ( dOrtLen > 0 ? (dDotd - dX * vtNd.x - dY * vtNd.y) / vtNd.z : (dDots - dX * vtNs.x - dY * vtNs.y) / vtNs.z) ;
|
|
|
|
|
|
|
|
if ( dProj1 < dPLen) { // Limiti su dOrtLen già imposti dall' if esterno
|
|
|
|
double dH = sqrt( dMaxRad * dMaxRad - dOrtLen* dOrtLen) ;
|
|
|
|
dMin = dZI - dH + ( dProj1 - dPLen) * dDeltaZ / dPLen ;
|
|
}
|
|
else if ( dProj1 >= dPLen) { // Limiti su dOrtLen e su dProj1 già imposti dall' if esterno
|
|
|
|
double dr = dMaxRad - ( dProj1 - dPLen) * dDeltaR / m_dTipHeight ;
|
|
|
|
double dH = sqrt( dr * dr - dOrtLen * dOrtLen) ;
|
|
|
|
dMax = dZI + dDeltaZ - dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMax, dMin) ;
|
|
}
|
|
else {
|
|
// dRatio >
|
|
}
|
|
}
|
|
else { // dDeltaZ > 0
|
|
|
|
if () { // dRatio <
|
|
|
|
}
|
|
else { // dRatio >
|
|
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
*/
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::XYLongVertConus( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
double dZL1, dZL2 ;
|
|
unsigned int nStartI, nEndI, nStartJ, nEndJ ;
|
|
|
|
bool Control = BoundingBox( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
// Parte cilindrica
|
|
m_nToolType = CylindricalMill ;
|
|
|
|
double dSafeHeight = m_dHeight ;
|
|
double dSafeTipHeight = m_dTipHeight ;
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
|
|
m_dHeight = dCylH ;
|
|
m_dTipHeight = 0 ;
|
|
|
|
XYLongVertCylBall( ptLs, ptLe, vtToolDir) ;
|
|
|
|
m_nToolType = ConusMill ;
|
|
m_dHeight = dSafeHeight ;
|
|
m_dTipHeight = dSafeTipHeight ;
|
|
|
|
// Parte conica
|
|
|
|
Point3d ptI, ptF ;
|
|
Vector3d vtV1 ;
|
|
|
|
double dMinRad = min( m_dRadius, m_dTipRadius) ;
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
double dDeltaR = dMaxRad - dMinRad ;
|
|
|
|
double dStem ;
|
|
|
|
if ( m_dRadius > m_dTipRadius) {
|
|
|
|
vtV1 = vtToolDir ;
|
|
dStem = - dCylH ;
|
|
}
|
|
else {
|
|
vtV1 = - vtToolDir ;
|
|
dStem = m_dHeight ;
|
|
}
|
|
|
|
if ( vtV1 * ( ptLe - ptLs) * ( ptLe.z - ptLs.z) > 0) {
|
|
|
|
ptI = ( ptLe.z - ptLs.z < 0 ? ptLs + vtV1 * dStem : ptLe + vtV1 * dStem) ;
|
|
ptF = ( ptLe.z - ptLs.z < 0 ? ptLe + vtV1 * dStem : ptLs + vtV1 * dStem) ;
|
|
}
|
|
|
|
else {
|
|
ptI = ( ptLe.z - ptLs.z > 0 ? ptLs + vtV1 * dStem : ptLe + vtV1 * dStem) ;
|
|
ptF = ( ptLe.z - ptLs.z > 0 ? ptLe + vtV1 * dStem : ptLs + vtV1 * dStem) ;
|
|
}
|
|
|
|
double dZI = ptI.z ;
|
|
double dDeltaZ = ptF.z - ptI.z ;
|
|
double dFactor = ( dDeltaZ > 0 ? - 1 : 1) ;
|
|
|
|
Vector3d vtV2 = vtV1 ; vtV2.Rotate(Z_AX, 90) ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
Vector3d vtMove = ptF - ptI ;
|
|
Vector3d vtMoveXY( vtMove.x, vtMove.y, 0) ; double dPLen = vtMoveXY.LenXY() ;
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
|
|
Point3d ptV = ptI - vtV1 * ( ( dMaxRad * m_dTipHeight) / dDeltaR) ;
|
|
|
|
// Apertura del cono e parametri per determinare i piani
|
|
double dTan = ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dRatio = ( dDeltaZ > 0 ? - ( vtMove * vtV1) / ( vtMove * vtV3) : ( vtMove * vtV1) / ( vtMove * vtV3)) ;
|
|
|
|
double dCos = dTan * dRatio ;
|
|
double dSin = ( 1 - dCos * dCos > 0 ? sqrt( 1 - dCos * dCos) : 0) ;
|
|
|
|
double dDen = sqrt( 1 + dTan * dTan) ;
|
|
|
|
// Versori normali e prodotti scalari per per determinare i piani
|
|
Vector3d vtNs = - ( dTan / dDen) * vtV1 + dFactor * ( dCos / dDen) * vtV3 - dFactor * ( dSin / dDen) * vtV2 ;
|
|
Vector3d vtNd = - ( dTan /dDen) * vtV1 + dFactor * ( dCos / dDen) * vtV3 + dFactor * ( dSin / dDen) * vtV2 ;
|
|
Vector3d vtR0 = ptV - ORIG ;
|
|
double dDots = vtR0 * vtNs ;
|
|
double dDotd = vtR0 * vtNd ;
|
|
|
|
|
|
// Ciclo
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dProj1 = - vtC * vtV1 ; double dProj2 = vtC * vtV2 ; double dOrtLen = abs( dProj2) ;
|
|
|
|
if ( ( dProj1 > 0 && dProj1 < dPLen && dOrtLen < dMaxRad) ||
|
|
( dProj1 >= dPLen && dProj1 < dPLen + m_dTipHeight &&
|
|
dOrtLen < dMaxRad + ( dProj1 - dPLen) * ( dDeltaR / m_dTipHeight))) {
|
|
|
|
|
|
if ( dRatio <= 1 / dTan) {
|
|
|
|
if ( dProj1 < m_dTipHeight && dOrtLen < dMaxRad * dSin - dProj1 * dDeltaR * dSin / m_dTipHeight) { // Il limite inferiore d dProj1 è già stato imposto dall' if più esterno
|
|
|
|
double dr = dMaxRad - dProj1 * dDeltaR / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - dOrtLen * dOrtLen) ;
|
|
|
|
dZL1 = dZI + dFactor * dH ;
|
|
}
|
|
else if ( dProj1 > 0 && dProj1 < dPLen &&
|
|
dOrtLen > dMaxRad * dSin && dOrtLen < dMaxRad) {
|
|
|
|
double dH = sqrt( dMaxRad * dMaxRad - dOrtLen * dOrtLen) ;
|
|
|
|
dZL1 = dZI + dFactor * dH + dProj1 * dDeltaZ / dPLen ;
|
|
}
|
|
else if ( dProj1 > m_dTipHeight && dProj1 < dPLen + m_dHeight &&
|
|
dOrtLen < dMinRad * dSin) {
|
|
|
|
double dH = sqrt( dMinRad * dMinRad - dOrtLen * dOrtLen) ;
|
|
|
|
dZL1 = dZI + dFactor * dH + ( dProj1 - m_dTipHeight) * dDeltaZ / dPLen ;
|
|
}
|
|
else if ( dProj1 >= dPLen &&
|
|
dOrtLen > dMaxRad * dSin - ( dProj1 - dPLen) * dDeltaR * dSin / m_dTipHeight) { // I limiti superiori sono già stati imposti dall' if più esterno
|
|
|
|
double dr = dMaxRad - ( dProj1 - dPLen) * dDeltaR / m_dTipHeight ;
|
|
|
|
double dH = sqrt( dr * dr - dOrtLen * dOrtLen) ;
|
|
|
|
dZL1 = dZI + dDeltaZ + dFactor * dH ;
|
|
}
|
|
else {
|
|
|
|
if ( dDeltaZ < 0)
|
|
|
|
dZL1 = ( dProj2 > 0 ? (dDotd - dX * vtNd.x - dY * vtNd.y) / vtNd.z : (dDots - dX * vtNs.x - dY * vtNs.y) / vtNs.z) ;
|
|
else
|
|
dZL1 = ( dProj2 > 0 ? (dDots - dX * vtNs.x - dY * vtNs.y) / vtNs.z : (dDotd - dX * vtNd.x - dY * vtNd.y) / vtNd.z) ;
|
|
}
|
|
|
|
|
|
|
|
if ( dProj1 < dPLen) { // Limiti su dOrtLen già imposti dall' if esterno
|
|
|
|
double dH = sqrt( dMaxRad * dMaxRad - dOrtLen* dOrtLen) ;
|
|
|
|
dZL2 = dZI - dFactor * dH + dProj1 * dDeltaZ / dPLen ;
|
|
}
|
|
else if ( dProj1 >= dPLen) { // Limiti su dOrtLen e su dProj1 già imposti dall' if esterno
|
|
|
|
double dr = dMaxRad - ( dProj1 - dPLen) * dDeltaR / m_dTipHeight ;
|
|
|
|
double dH = sqrt( dr * dr - dOrtLen * dOrtLen) ;
|
|
|
|
dZL2 = dZI + dDeltaZ - dFactor * dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dZL1, dZL2) ;
|
|
}
|
|
else {
|
|
|
|
if ( dProj1 < dPLen && dOrtLen < dMaxRad) {
|
|
|
|
double dH = sqrt( dMaxRad * dMaxRad - dOrtLen * dOrtLen) ;
|
|
|
|
dZL1 = dZI + dH + dProj1 * dDeltaZ / dPLen ;
|
|
dZL2 = dZI - dH + dProj1 * dDeltaZ / dPLen ;
|
|
}
|
|
else if ( dProj1 >= dPLen) {
|
|
|
|
double dr = dMaxRad - ( dProj1 - dPLen) * dDeltaR / m_dTipHeight ;
|
|
|
|
double dH = sqrt( dr * dr - dOrtLen * dOrtLen) ;
|
|
|
|
dZL1 = dZI + dDeltaZ + dH ;
|
|
dZL2 = dZI + dDeltaZ - dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dZL1, dZL2) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingXY( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
|
|
if ( m_nToolType == CylindricalMill)
|
|
|
|
return MillingXYCyl( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else if ( m_nToolType == BallEndMill)
|
|
|
|
return MillingXYBall( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else if ( m_nToolType == ConusMill)
|
|
|
|
return MillingXYConus( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else
|
|
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingXYCyl( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
double dMinZ = min( ptLs.z, ptLe.z) - m_dRadius ;
|
|
double dMaxZ = max( ptLs.z, ptLe.z) + m_dRadius ;
|
|
|
|
// Prima verifica sull'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return true ;
|
|
|
|
Point3d ptI, ptF ;
|
|
|
|
if ( ptLs.z < ptLe.z) {
|
|
ptI = ptLs ;
|
|
ptF = ptLe ;
|
|
}
|
|
else {
|
|
ptI = ptLe ;
|
|
ptF = ptLs ;
|
|
}
|
|
|
|
// Quote dei punti ptI e ptF
|
|
double dZI = ptI.z ; double dZF = ptF.z ;
|
|
|
|
Point3d ptIT = ptI - vtToolDir * m_dHeight ;
|
|
Point3d ptFT = ptF - vtToolDir * m_dHeight ;
|
|
|
|
// Bounding box
|
|
double dMinX = min( min( ptI.x, ptIT.x), min( ptF.x, ptFT.x)) - m_dRadius ;
|
|
double dMaxX = max( max( ptI.x, ptIT.x), max( ptF.x, ptFT.x)) + m_dRadius ;
|
|
double dMinY = min( min( ptI.y, ptIT.y), min( ptF.y, ptFT.y)) - m_dRadius ;
|
|
double dMaxY = max( max( ptI.y, ptIT.y), max( ptF.y, ptFT.y)) + m_dRadius ;
|
|
|
|
// Seconda verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
|
|
Vector3d vtMove = ptF - ptI ; //double dLenPath = vtMove.Len() ;
|
|
|
|
// Determino sistema di riferimento del movimento: costruisco un sistrema di vettori ortonormali e destrorsi spiccati dal punto iniziale del movimento
|
|
Vector3d vtV1 = vtToolDir ;
|
|
Vector3d vtV2 = vtMove ; vtV2.Normalize() ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
|
|
// Determinazione punti notevoli del volume spazzato dall'utensile
|
|
Point3d ptPlaneSup, ptPlaneInf ;
|
|
|
|
if ( vtV3.z > 0) {
|
|
ptPlaneInf = ptI - m_dRadius * vtV3 ;
|
|
ptPlaneSup = ptI + m_dRadius * vtV3 ;
|
|
}
|
|
else {
|
|
ptPlaneInf = ptI + m_dRadius * vtV3 ;
|
|
ptPlaneSup = ptI - m_dRadius * vtV3 ;
|
|
}
|
|
|
|
// Prodotti scalari per costruire i piani passanti per i punti notevoli
|
|
Vector3d vtR0Inf = ptPlaneInf - ORIG ;
|
|
Vector3d vtR0Sup = ptPlaneSup - ORIG ;
|
|
|
|
double dPInf = vtR0Inf * vtV3 ; double dPSup = vtR0Sup * vtV3 ;
|
|
|
|
// Determinazione delle proiezioni sul piano delle entità geometriche fondamentali
|
|
Vector3d vtPlaneMove( vtMove.x, vtMove.y, 0) ; double dPlanePath = vtPlaneMove.Len() ; vtPlaneMove.Normalize() ;
|
|
Vector3d vtPlaneLim( m_dRadius * vtV3.x, m_dRadius * vtV3.y, 0) ; double dPlaneLim = vtPlaneLim.Len() ;
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
|
|
Vector3d vtTemp = vtV1 ; vtTemp.Rotate( Z_AX, 90) ;
|
|
Vector3d vtW2 = ( vtTemp * vtV2 > 0 ? vtTemp : - vtTemp) ;
|
|
|
|
// Determinazione limiti sugli indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
// Ciclo
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dMin, dMax ;
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
double dZInf = ( dPInf - vtV3.x * dX - vtV3.y * dY) / vtV3.z ; // Quota Z Piano inferiore come funzione di x e y
|
|
double dZSup = ( dPSup - vtV3.x * dX - vtV3.y * dY) / vtV3.z ; // Quota Z Piano inferiore come funzione di x e y
|
|
|
|
Point3d ptC( dX, dY, 0) ;
|
|
Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dProj1 = vtC * vtV1 ; // vtV1 è vtToolDir che per il momento giace nel piano
|
|
double dProj2 = vtC * vtW2 ; // vtPlaneMove è stato normalizzato dopo averne calcolato la lunghezza
|
|
|
|
if ( dProj1 < 0 && dProj1 > - m_dHeight && dProj2 > - m_dRadius && dProj2 < dPlanePath + m_dRadius) {
|
|
|
|
|
|
// Minimi
|
|
if ( dProj2 > - m_dRadius && dProj2 < dPlaneLim) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dProj2 * dProj2) ;
|
|
|
|
dMin = dZI - dH ;
|
|
}
|
|
else if ( dProj2 >= dPlaneLim && dProj2 < dPlanePath + dPlaneLim) {
|
|
|
|
dMin = dZInf ;
|
|
}
|
|
else if ( dProj2 >= dPlanePath + dPlaneLim && dProj2 < dPlanePath + m_dRadius) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - ( dProj2 - dPlanePath) * ( dProj2 - dPlanePath)) ;
|
|
|
|
dMin = dZF - dH ;
|
|
}
|
|
// Massimi
|
|
if ( dProj2 > - m_dRadius && dProj2 < - dPlaneLim) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dProj2 * dProj2) ;
|
|
|
|
dMax = dZI + dH ;
|
|
}
|
|
else if ( dProj2 >= - dPlaneLim && dProj2 < dPlanePath - dPlaneLim) {
|
|
|
|
dMax = dZSup ;
|
|
}
|
|
else if ( dProj2 >= dPlanePath - dPlaneLim && dProj2 < dPlanePath + m_dRadius) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - ( dProj2 - dPlanePath) * ( dProj2 - dPlanePath)) ;
|
|
|
|
dMax = dZF + dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingXYBall( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
double dMinZ = min( ptLs.z, ptLe.z) - m_dRadius ;
|
|
double dMaxZ = max( ptLs.z, ptLe.z) + m_dRadius ;
|
|
|
|
// Prima verifica sull'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return true ;
|
|
|
|
Point3d ptI, ptF ;
|
|
|
|
if ( ptLs.z < ptLe.z) {
|
|
ptI = ptLs ;
|
|
ptF = ptLe ;
|
|
}
|
|
else {
|
|
ptI = ptLe ;
|
|
ptF = ptLs ;
|
|
}
|
|
|
|
// Quote dei punti ptI e ptF e DeltaZ
|
|
double dZI = ptI.z ; double dZF = ptF.z ; double dDeltaZ = dZF - dZI ;
|
|
|
|
Point3d ptIT = ptI - vtToolDir * m_dHeight ;
|
|
Point3d ptFT = ptF - vtToolDir * m_dHeight ;
|
|
|
|
// Bounding box
|
|
double dMinX = min( min( ptI.x, ptIT.x), min( ptF.x, ptFT.x)) - m_dRadius ;
|
|
double dMaxX = max( max( ptI.x, ptIT.x), max( ptF.x, ptFT.x)) + m_dRadius ;
|
|
double dMinY = min( min( ptI.y, ptIT.y), min( ptF.y, ptFT.y)) - m_dRadius ;
|
|
double dMaxY = max( max( ptI.y, ptIT.y), max( ptF.y, ptFT.y)) + m_dRadius ;
|
|
|
|
// Seconda verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
|
|
Vector3d vtMove = ptF - ptI ;
|
|
|
|
// Determino sistema di riferimento del movimento: costruisco un sistrema di vettori ortonormali e destrorsi spiccati dal punto iniziale del movimento
|
|
Vector3d vtV1 = vtToolDir ;
|
|
Vector3d vtV2 = vtMove ; vtV2.Normalize() ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
|
|
// Determinazione punti notevoli del volume spazzato dall'utensile
|
|
Point3d ptPlaneSup, ptPlaneInf ;
|
|
|
|
if ( vtV3.z > 0) {
|
|
ptPlaneInf = ptI - m_dRadius * vtV3 ;
|
|
ptPlaneSup = ptI + m_dRadius * vtV3 ;
|
|
}
|
|
else {
|
|
ptPlaneInf = ptI + m_dRadius * vtV3 ;
|
|
ptPlaneSup = ptI - m_dRadius * vtV3 ;
|
|
}
|
|
|
|
// Prodotti scalari per costruire i piani passanti per i punti notevoli
|
|
Vector3d vtR0Inf = ptPlaneInf - ORIG ;
|
|
Vector3d vtR0Sup = ptPlaneSup - ORIG ;
|
|
|
|
double dPInf = vtR0Inf * vtV3 ; double dPSup = vtR0Sup * vtV3 ;
|
|
|
|
// Determinazione delle proiezioni sul piano delle entità geometriche fondamentali
|
|
Vector3d vtPlaneMove( vtMove.x, vtMove.y, 0) ; double dPlanePath = vtPlaneMove.Len() ; vtPlaneMove.Normalize() ;
|
|
Vector3d vtPlaneLim( m_dRadius * vtV3.x, m_dRadius * vtV3.y, 0) ; double dPlaneLim = vtPlaneLim.Len() ;
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
|
|
Vector3d vtTemp = vtToolDir ; vtTemp.Rotate( Z_AX, 90) ;
|
|
Vector3d vtW2 = ( vtTemp * vtV2 > 0 ? vtTemp : - vtTemp) ;
|
|
|
|
// Determinazione limiti sugli indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
// Ciclo
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dMin, dMax ;
|
|
double dCylH = m_dHeight - m_dRadius ;
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
double dZInf = ( dPInf - vtV3.x * dX - vtV3.y * dY) / vtV3.z ; // Quota Z Piano inferiore come funzione di x e y
|
|
double dZSup = ( dPSup - vtV3.x * dX - vtV3.y * dY) / vtV3.z ; // Quota Z Piano inferiore come funzione di x e y
|
|
|
|
Point3d ptC( dX, dY, 0) ;
|
|
Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dProj1 = vtC * vtV1 ; // vtV1 è vtToolDir che per il momento giace nel piano
|
|
double dProj2 = vtC * vtW2 ; // vtPlaneMove è stato normalizzato dopo averne calcolato la lunghezza
|
|
|
|
// Parte cilindrica
|
|
if ( dProj1 < 0 && dProj1 > - dCylH && dProj2 > - m_dRadius && dProj2 < dPlanePath + m_dRadius) {
|
|
|
|
// Minimi
|
|
if ( dProj2 > - m_dRadius && dProj2 < dPlaneLim) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dProj2 * dProj2) ;
|
|
|
|
dMin = dZI - dH ;
|
|
}
|
|
else if ( dProj2 >= dPlaneLim && dProj2 < dPlanePath + dPlaneLim) {
|
|
|
|
dMin = dZInf ;
|
|
}
|
|
else if ( dProj2 >= dPlanePath + dPlaneLim && dProj2 < dPlanePath + m_dRadius) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - ( dProj2 - dPlanePath) * ( dProj2 - dPlanePath)) ;
|
|
|
|
dMin = dZF - dH ;
|
|
}
|
|
// Massimi
|
|
if ( dProj2 > - m_dRadius && dProj2 < - dPlaneLim) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dProj2 * dProj2) ;
|
|
|
|
dMax = dZI + dH ;
|
|
}
|
|
else if ( dProj2 >= - dPlaneLim && dProj2 < dPlanePath - dPlaneLim) {
|
|
|
|
dMax = dZSup ;
|
|
}
|
|
else if ( dProj2 >= dPlanePath - dPlaneLim && dProj2 < dPlanePath + m_dRadius) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - ( dProj2 - dPlanePath) * ( dProj2 - dPlanePath)) ;
|
|
|
|
dMax = dZF + dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
// Parte sferica
|
|
else if ( dProj1 <= - dCylH && ( ( dProj2 > - m_dRadius && dProj2 < 0 && dProj2 * dProj2 + (- dProj1 - dCylH) * (- dProj1 - dCylH) < m_dRadius * m_dRadius) ||
|
|
( dProj2 >= 0 && dProj2 < dPlanePath && dProj1 > - m_dHeight) ||
|
|
( dProj2 >= dPlanePath && dProj2 < dPlanePath + m_dRadius && ( dProj2 - dPlanePath) * ( dProj2 - dPlanePath) + (- dProj1 - dCylH) * (- dProj1 - dCylH) < m_dRadius * m_dRadius))) {
|
|
|
|
|
|
double dSemiMin = dPlaneLim ; double dSemiMax = m_dRadius ; // Semi-assi dell'ellisse
|
|
double dl = - dProj1 - dCylH ;
|
|
|
|
// Massimi
|
|
if ( dProj2 < - dSemiMin * sqrt( 1 - ( dl * dl) / ( dSemiMax * dSemiMax))) {
|
|
|
|
double dr = sqrt( dProj2 * dProj2 + dl * dl) ;
|
|
double dH = sqrt( m_dRadius * m_dRadius - dr * dr) ;
|
|
|
|
dMax = dZI + dH ;
|
|
}
|
|
else if ( dProj2 < dPlanePath - dSemiMin * sqrt( 1 - ( dl * dl) / ( dSemiMax * dSemiMax))) {
|
|
|
|
double dCos = abs( vtV3 * Z_AX) ;
|
|
|
|
dMax = dZI + sqrt( m_dRadius * m_dRadius - dl * dl) * dCos + ( dDeltaZ / dPlanePath) * ( dProj2 + dSemiMin * sqrt( 1 - ( dl * dl) / ( dSemiMax * dSemiMax))) ;
|
|
}
|
|
else {
|
|
|
|
double dL = dProj2 - dPlanePath ;
|
|
double dr = sqrt( dL * dL + dl * dl) ;
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dr * dr) ;
|
|
|
|
dMax = dZF + dH ;
|
|
}
|
|
|
|
// Minimi
|
|
if ( dProj2 < dSemiMin * sqrt( 1 - ( dl * dl) / ( dSemiMax * dSemiMax))) {
|
|
|
|
double dr = sqrt( dProj2 * dProj2 + dl * dl) ;
|
|
double dH = sqrt( m_dRadius * m_dRadius - dr * dr) ;
|
|
|
|
dMin = dZI - dH ;
|
|
}
|
|
else if ( dProj2 < dPlanePath + dSemiMin * sqrt( 1 - ( dl * dl) / ( dSemiMax * dSemiMax))) {
|
|
|
|
double dCos = abs( vtV3 * Z_AX) ;
|
|
|
|
dMin = dZI - sqrt( m_dRadius * m_dRadius - dl * dl) * dCos + ( dDeltaZ / dPlanePath) * ( dProj2 - dSemiMin * sqrt( 1 - ( dl * dl) / ( dSemiMax * dSemiMax))) ;
|
|
}
|
|
else {
|
|
|
|
double dr = sqrt( ( dProj2 - dPlanePath) * ( dProj2 - dPlanePath) + dl * dl) ;
|
|
double dH = sqrt( m_dRadius * m_dRadius - dr * dr) ;
|
|
|
|
dMin = dZF - dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingXYConus( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
double dMin, dMax ;
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BoundingBox( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
|
|
// Parte cilindrica
|
|
m_nToolType = CylindricalMill ;
|
|
m_dHeight = dCylH ;
|
|
|
|
MillingXY( ptLs, ptLe, vtToolDir) ;
|
|
|
|
m_nToolType = ConusMill ;
|
|
m_dHeight = m_dHeight + m_dTipHeight ;
|
|
|
|
Vector3d vtV1, vtV2, vtV3 ;
|
|
Point3d ptI, ptF ;
|
|
|
|
double dStem ;
|
|
|
|
double dMinRad = min( m_dRadius, m_dTipRadius) ;
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
double dDeltaR = dMaxRad - dMinRad ;
|
|
|
|
// Studio della parte conica
|
|
if ( m_dRadius > m_dTipRadius) {
|
|
|
|
vtV1 = vtToolDir ;
|
|
dStem = - dCylH ;
|
|
}
|
|
else {
|
|
|
|
vtV1 = - vtToolDir ;
|
|
dStem = m_dHeight ;
|
|
}
|
|
|
|
ptI = ( ptLs.z < ptLe.z ? ptLs + vtV1 * dStem : ptLe + vtV1 * dStem) ;
|
|
ptF = ( ptLs.z < ptLe.z ? ptLe + vtV1 * dStem : ptLs + vtV1 * dStem) ;
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ; double dZI = ptI.z ; double dDeltaZ = ptF.z - ptI.z ;
|
|
|
|
Vector3d vtMove = ptF - ptI ;
|
|
Vector3d vtMoveXY( vtMove.x, vtMove.y, 0) ; double dPLen = vtMoveXY.LenXY() ;
|
|
Vector3d vtU2 = vtV1 ; vtU2.Rotate(Z_AX, 90) ;
|
|
|
|
if ( vtMoveXY * vtU2 < 0)
|
|
|
|
vtU2 = - vtU2 ;
|
|
|
|
vtV2 = vtMove ; vtV2.Normalize() ;
|
|
vtV3 = vtV1 ^ vtV2 ;
|
|
|
|
Point3d ptV = ptI - vtV1 * ( dMaxRad * m_dTipHeight / ( dMaxRad - dMinRad)) ;
|
|
|
|
Vector3d vtV3XY( vtV3.x, vtV3.y, 0) ;
|
|
|
|
double dPrV3 = vtV3XY.LenXY() ;
|
|
double dRl = dMaxRad * dPrV3 ;
|
|
double drl = dMinRad * dPrV3 ;
|
|
double dDl = dDeltaR * dPrV3 ;
|
|
|
|
// Apertura del cono e parametri per determinare i piani
|
|
double dTanAlpha = ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dRatio = ( vtMove * vtV1) / ( vtMove * vtV2) ;
|
|
|
|
double dCos = dTanAlpha * dRatio ;
|
|
double dSin = ( 1 - dCos * dCos > 0 ? sqrt( 1 - dCos * dCos) : 0) ;
|
|
|
|
double dDen = sqrt( 1 + dTanAlpha * dTanAlpha) ;
|
|
|
|
// Versori normali e prodotti scalari per per determinare i piani
|
|
Vector3d vtNs = - ( dTanAlpha / dDen) * vtV1 + ( dCos / dDen) * vtV2 + ( dSin / dDen) * vtV3 ;
|
|
Vector3d vtNd = - ( dTanAlpha / dDen) * vtV1 + ( dCos / dDen) * vtV2 - ( dSin / dDen) * vtV3 ;
|
|
|
|
Vector3d vtR0 = ptV - ORIG ;
|
|
double dDots = vtR0 * vtNs ;
|
|
double dDotd = vtR0 * vtNd ;
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dProj1 = - vtC * vtV1 ; double dProj2 = vtC * vtU2 ;
|
|
|
|
if ( dProj1 > 0 && dProj1 < m_dTipHeight &&
|
|
dProj2 > - dMaxRad + dProj1 * dDeltaR / m_dTipHeight &&
|
|
dProj2 < dPLen + dMaxRad - dProj1 * dDeltaR / m_dTipHeight) {
|
|
/*
|
|
if ( dProj2 < - dRl + dProj1 * dDl / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad - dProj1 * dDeltaR / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - dProj2 * dProj2) ;
|
|
|
|
dMin = dZI - dH ;
|
|
dMax = dZI + dH ;
|
|
}
|
|
else if ( dProj2 >= - dRl + dProj1 * dDl / m_dTipHeight &&
|
|
dProj2 < dRl - dProj1 * dDl / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad - dProj1 * dDeltaR / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - dProj2 * dProj2) ;
|
|
|
|
dMin = dZI - dH ;
|
|
dMax = ( dDotd - dX * vtNd.x - dY * vtNd.y) / vtNd.z ;
|
|
}
|
|
else if ( dProj2 >= dRl - dProj1 * dDl / m_dTipHeight &&
|
|
dProj2 < dPLen - dRl + dProj1 * dDl / m_dTipHeight) {
|
|
|
|
dMin = ( dDots - dX * vtNs.x - dY * vtNs.y) / vtNs.z ;
|
|
dMax = ( dDotd - dX * vtNd.x - dY * vtNd.y) / vtNd.z ;
|
|
}
|
|
else if ( dProj2 >= dPLen - dRl + dProj1 * dDl / m_dTipHeight &&
|
|
dProj2 < dPLen + dRl - dProj1 * dDl / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad - dProj1 * dDeltaR / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - ( dProj2 - dPLen) * ( dProj2 - dPLen)) ;
|
|
|
|
dMin = ( dDots - dX * vtNs.x - dY * vtNs.y) / vtNs.z ;
|
|
dMax = dZI + dDeltaZ + dH ;
|
|
}
|
|
else {
|
|
|
|
double dr = dMaxRad - dProj1 * dDeltaR / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - ( dProj2 - dPLen) * ( dProj2 - dPLen)) ;
|
|
|
|
dMin = dZI + dDeltaZ - dH ;
|
|
dMax = dZI + dDeltaZ + dH ;
|
|
} */
|
|
|
|
// Massimi
|
|
if ( dProj2 < - dRl + dProj1 * dDl / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad - dProj1 * dDeltaR / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - dProj2 * dProj2) ;
|
|
|
|
dMax = dZI + dH ;
|
|
}
|
|
else if ( dProj2 >= - dRl + dProj1 * dDl / m_dTipHeight &&
|
|
dProj2 < dPLen - dRl + dProj1 * dDl / m_dTipHeight)
|
|
|
|
dMax = ( vtV3.z < 0 ? ( dDotd - dX * vtNd.x - dY * vtNd.y) / vtNd.z : ( dDots - dX * vtNs.x - dY * vtNs.y) / vtNs.z) ;
|
|
else {
|
|
|
|
double dr = dMaxRad - dProj1 * dDeltaR / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - ( dProj2 - dPLen) * ( dProj2 - dPLen)) ;
|
|
|
|
dMax = dZI + dDeltaZ + dH ;
|
|
}
|
|
|
|
// Minimi
|
|
if ( dProj2 < dRl - dProj1 * dDl / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad - dProj1 * dDeltaR / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - dProj2 * dProj2) ;
|
|
|
|
dMin = dZI - dH ;
|
|
}
|
|
else if ( dProj2 >= dRl - dProj1 * dDl / m_dTipHeight &&
|
|
dProj2 < dPLen + dRl - dProj1 * dDl / m_dTipHeight)
|
|
|
|
dMin = ( vtV3.z < 0 ? ( dDots - dX * vtNs.x - dY * vtNs.y) / vtNs.z : ( dDotd - dX * vtNd.x - dY * vtNd.y) / vtNd.z) ;
|
|
|
|
else {
|
|
|
|
double dr = dMaxRad - dProj1 * dDeltaR / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - ( dProj2 - dPLen) * ( dProj2 - dPLen)) ;
|
|
|
|
dMin = dZI + dDeltaZ - dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingXYPlus( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir) {
|
|
|
|
if ( m_nToolType == CylindricalMill)
|
|
|
|
return MillingXYPlusCyl( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else if ( m_nToolType == BallEndMill)
|
|
|
|
return MillingXYPlusBall( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else if ( m_nToolType == ConusMill)
|
|
|
|
return MillingXYPlusConus( ptLs, ptLe, vtToolDir) ;
|
|
|
|
else
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingXYPlusCyl( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
double dMinZ = min( ptLs.z, ptLe.z) - m_dRadius ;
|
|
double dMaxZ = max( ptLs.z, ptLe.z) + m_dRadius ;
|
|
|
|
// Prima verifica sull'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return true ;
|
|
|
|
Point3d ptI, ptF ;
|
|
|
|
if ( ptLs.z <= ptLe.z) {
|
|
ptI = ptLs ;
|
|
ptF = ptLe ;
|
|
}
|
|
else {
|
|
ptI = ptLe ;
|
|
ptF = ptLs ;
|
|
}
|
|
|
|
// Quote dei punti ptI e ptF e DeltaZ
|
|
double dZI = ptI.z ; double dZF = ptF.z ; double dDeltaZ = dZF - dZI ;
|
|
|
|
Point3d ptIT = ptI - vtToolDir * m_dHeight ;
|
|
Point3d ptFT = ptF - vtToolDir * m_dHeight ;
|
|
|
|
// Bounding box
|
|
double dMinX = min( min( ptI.x, ptIT.x), min( ptF.x, ptFT.x)) - m_dRadius ;
|
|
double dMaxX = max( max( ptI.x, ptIT.x), max( ptF.x, ptFT.x)) + m_dRadius ;
|
|
double dMinY = min( min( ptI.y, ptIT.y), min( ptF.y, ptFT.y)) - m_dRadius ;
|
|
double dMaxY = max( max( ptI.y, ptIT.y), max( ptF.y, ptFT.y)) + m_dRadius ;
|
|
|
|
// Seconda verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
|
|
// Vettori di riferimento nello spazio e controllo per simmetria
|
|
Vector3d vtMove = ptF - ptI ;
|
|
Vector3d vtTool = vtToolDir ;
|
|
|
|
if ( vtToolDir * vtMove > 0) {
|
|
|
|
Point3d ptTemp = ptI ;
|
|
ptI = ptIT ; ptIT = ptTemp ;
|
|
ptTemp = ptF ;
|
|
ptF = ptFT ;
|
|
ptFT = ptTemp ;
|
|
vtTool = - vtTool ;
|
|
}
|
|
|
|
Vector3d vtMoveLong = ( vtMove * vtTool) * vtTool ; double dLen1 = vtMoveLong.Len() ;
|
|
Vector3d vtMoveOrt = vtMove - vtMoveLong ; double dLen2 = vtMoveOrt.Len() ;
|
|
|
|
// Vettori di riferimento nel piano
|
|
Vector3d vtPlaneMove( vtMove.x, vtMove.y, 0) ; double dPLen = vtPlaneMove.LenXY() ;
|
|
Vector3d vtPlaneMoveLong( vtMoveLong.x, vtMoveLong.y, 0) ; double dPLen1 = vtPlaneMoveLong.LenXY() ;
|
|
Vector3d vtPlaneMoveOrt( vtMoveOrt.x, vtMoveOrt.y, 0) ; double dPLen2 = vtPlaneMoveOrt.LenXY() ; vtPlaneMoveOrt.Normalize() ;
|
|
|
|
// Punti iniziale e finale proiettati sul piano
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ; Point3d ptFxy( ptF.x, ptF.y, 0) ;
|
|
|
|
// Determino i sistemi di riferimento del movimento: costruisco un sistrema di vettori ortonormali e destrorsi spiccati dal punto iniziale del movimento
|
|
Vector3d vtV1 = vtTool ;
|
|
Vector3d vtV2 = vtMoveOrt ; vtV2.Normalize() ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
|
|
// Determinazione punti notevoli del volume spazzato dall'utensile
|
|
Point3d ptPlaneSup, ptPlaneInf ;
|
|
|
|
if ( vtV3.z > 0) {
|
|
ptPlaneInf = ptI - m_dRadius * vtV3 ;
|
|
ptPlaneSup = ptI + m_dRadius * vtV3 ;
|
|
}
|
|
else {
|
|
ptPlaneInf = ptI + m_dRadius * vtV3 ;
|
|
ptPlaneSup = ptI - m_dRadius * vtV3 ;
|
|
}
|
|
|
|
Point3d ptPlaneSupxy( ptPlaneSup.x, ptPlaneSup.y, 0) ;
|
|
Point3d ptPlaneInfxy( ptPlaneInf.x, ptPlaneInf.y, 0) ;
|
|
|
|
double dr = sqrt( ( ptPlaneSupxy - ptIxy) * ( ptPlaneSupxy - ptIxy)) ;
|
|
|
|
// Determinazione degli analoghi punti sulla punta dell'utensile e delle loro proiezioni sul piano XY
|
|
Point3d ptPlTInf = ptPlaneInf - m_dHeight * vtTool ; Point3d ptPlTInfxy( ptPlTInf.x, ptPlTInf.y, 0) ;
|
|
Point3d ptPlTSup = ptPlaneSup - m_dHeight * vtTool ; Point3d ptPlTSupxy( ptPlTSup.x, ptPlTSup.y, 0) ;
|
|
|
|
// Prodotti scalari per costruire i piani passanti per i punti notevoli
|
|
Vector3d vtR0Inf = ptPlaneInf - ORIG ;
|
|
Vector3d vtR0Sup = ptPlaneSup - ORIG ;
|
|
Vector3d vtR0 = ptI - ORIG ;
|
|
double dPInf = vtR0Inf * vtV3 ; double dPSup = vtR0Sup * vtV3 ; double dP = vtR0 * vtV3 ;
|
|
|
|
// Determinazione limiti sugli indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
// Ciclo
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dMin, dMax ;
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
double dZPInf = ( dPInf - vtV3.x * dX - vtV3.y * dY) / vtV3.z ; // Quota Z Piano inferiore come funzione di x e y
|
|
double dZPSup = ( dPSup - vtV3.x * dX - vtV3.y * dY) / vtV3.z ; // Quota Z Piano superiore come funzione di x e y
|
|
|
|
// Punto e vettori del ciclo
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtCi = ptC - ptIxy ;
|
|
// Proiezione fondamentale
|
|
double dPro1 = vtCi * vtV1 ; double dPro2 = vtCi * vtPlaneMoveOrt ;
|
|
|
|
|
|
// Se il punto cade nella proiezione sul piano XY del volume spazzato si taglia
|
|
if ( ((dPro2 > - m_dRadius && dPro2 < m_dRadius) && (((dPro2 < dPLen2 - m_dRadius && (dPro1 > - m_dHeight - (dPLen1 / dPLen2) * (dPro2 + m_dRadius) && dPro1 < 0))) || (dPro2 >= dPLen2 - m_dRadius && (dPro1 > - m_dHeight - dPLen1 && dPro1 < 0))))
|
|
|| ((dPro2 >= m_dRadius && dPro2 < dPLen2 + m_dRadius) && ((dPro2 < dPLen2 - m_dRadius && (dPro1 > - (dPLen1 / dPLen2) * (dPro2 + m_dRadius) - m_dHeight && dPro1 < - (dPLen1 / dPLen2) * (dPro2 - m_dRadius))) || (dPro2 >= dPLen2 - m_dRadius && (dPro1 > - m_dHeight - dPLen1 && dPro1 < - (dPLen1 / dPLen2) * (dPro2 - m_dRadius)))))) {
|
|
|
|
// Massimi //////////////////////////////////////////////////////////////////////////
|
|
// Prima zona cilindrica superiore
|
|
if ( ( dPro2 > - m_dRadius && dPro2 < - dr) && ( dPro1 > - m_dHeight && dPro1 < 0)) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dPro2 * dPro2) ;
|
|
|
|
dMax = dZI + dH ;
|
|
}
|
|
|
|
// Vettore per seconda zona cilindrica superiore
|
|
Vector3d vtCf = ptC - ptFxy ;
|
|
// Proiezione per seconda zona cilindrica sueriore
|
|
double dPr1 = vtCf * vtV1 ; double dPr2 = vtCf * vtPlaneMoveOrt ;
|
|
|
|
// Seconda zona cilindrica superiore
|
|
if ( ( dPr2 >= - dr && dPr2 < m_dRadius) && ( dPr1 > - m_dHeight && dPr1 <= 0)) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dPr2 * dPr2) ;
|
|
|
|
dMax = dZF + dH ;
|
|
}
|
|
|
|
// Vettore per Piano superiore e zona di fondo superiore
|
|
Vector3d vtCS = ptC - ptPlaneSupxy ;
|
|
// Proiezioni
|
|
double dPrS1 = vtCS * vtV1 ; double dPrS2 = vtCS * vtPlaneMoveOrt ;
|
|
|
|
// Piano superiore
|
|
if ( dPrS2 >= 0 && dPrS2 < dPLen2 && dPrS1 > - m_dHeight - ( dPLen1/dPLen2) * dPrS2 && dPrS1 <= - ( dPLen1/dPLen2) * dPrS2)
|
|
|
|
dMax = dZPSup ;
|
|
|
|
// Vettore per zona di punta superiore
|
|
Vector3d vtCTS = ptC - ptPlTSupxy ;
|
|
// Proiezioni
|
|
double dPrTS1 = vtCTS * vtV1 ; double dPrTS2 = vtCTS * vtPlaneMoveOrt ;
|
|
|
|
// Zona di punta superiore
|
|
if ( dPrTS1 <= 0 && dPrTS1 > - dPLen1)
|
|
if ( dPrTS2 <= - ( dPLen2 / dPLen1) * dPrTS1 && dPrTS2 > - ( m_dRadius - dr) - ( dPLen2 / dPLen1) * dPrTS1) {
|
|
|
|
double dDist = - ( dPLen2 / dPLen1) * dPrTS1 - dPrTS2 ;
|
|
double dL = dDist + dr ;
|
|
double dH = ( m_dRadius * m_dRadius - dL * dL > 0 ? sqrt( m_dRadius * m_dRadius - dL * dL) : 0) ;
|
|
double dl = sqrt( dPrTS1 * dPrTS1 + ( ( dPLen2 / dPLen1) * dPrTS1) * ( ( dPLen2 / dPLen1) * dPrTS1)) ;
|
|
|
|
dMax = dZI + ( dDeltaZ / dPLen) * dl + dH ;
|
|
}
|
|
|
|
// Zona di fondo superiore
|
|
if ( dPrS1 < 0 && dPrS1 > - dPLen1)
|
|
if ( dPrS2 > - ( dPLen2 / dPLen1) * dPrS1 && dPrS2 < m_dRadius + dr - ( dPLen2 / dPLen1) * dPrS1) {
|
|
|
|
double dL = abs( dr - ( dPLen2 / dPLen1) * dPrS1 - dPrS2) ;
|
|
double dH = ( m_dRadius * m_dRadius - dL * dL > 0 ? sqrt( m_dRadius * m_dRadius - dL * dL) : 0) ;
|
|
double dl = sqrt( dPrS1 * dPrS1 + ( ( dPLen2 / dPLen1) * dPrS1) * ( ( dPLen2 / dPLen1) * dPrS1)) ;
|
|
|
|
dMax = dZI + ( dDeltaZ / dPLen) * dl + dH ;
|
|
}
|
|
|
|
// Minimi //////////////////////////////////////////////////////////////////////////
|
|
// Prima zona cilindrica inferiore
|
|
if ( ( dPro2 > - m_dRadius && dPro2 < dr) && ( dPro1 > - m_dHeight && dPro1 < 0)) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dPro2 * dPro2) ;
|
|
|
|
dMin = dZI - dH ;
|
|
}
|
|
|
|
// Seconda zona cilindrica inferiore
|
|
if ( ( dPr2 >= dr && dPr2 < m_dRadius) && ( dPr1 > - m_dHeight && dPr1 <= 0)) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dPr2 * dPr2) ;
|
|
|
|
dMin = dZF - dH ;
|
|
}
|
|
|
|
// Vettore per piano inferiore e zona di fondo inferiore
|
|
Vector3d vtCI = ptC - ptPlaneInfxy ;
|
|
// Proiezioni
|
|
double dPrI1 = vtCI * vtV1 ; double dPrI2 = vtCI * vtPlaneMoveOrt ;
|
|
|
|
// Piano inferiore
|
|
if ( dPrI2 >= 0 && dPrI2 < dPLen2 && dPrI1 > - m_dHeight - ( dPLen1/dPLen2) * dPrI2 && dPrI1 <= - ( dPLen1/dPLen2) * dPrI2)
|
|
|
|
dMin = dZPInf ;
|
|
|
|
// Zona di fondo inferiore
|
|
if ( dPrI1 <= 0 && dPrI1 > - dPLen1)
|
|
if ( dPrI2 > - ( dPLen2 / dPLen1) * dPrI1 && dPrI2 < ( m_dRadius - dr) - ( dPLen2 / dPLen1) * dPrI1) {
|
|
|
|
double dDist = dPrI2 + ( dPLen2 / dPLen1) * dPrI1 ;
|
|
double dL = dDist + dr ;
|
|
double dH = ( m_dRadius * m_dRadius - dL * dL > 0 ? sqrt( m_dRadius * m_dRadius - dL * dL) : 0) ;
|
|
double dl = sqrt( dPrI1 * dPrI1 + ( ( dPLen2 / dPLen1) * dPrI1) * ( ( dPLen2 / dPLen1) * dPrI1)) ;
|
|
|
|
dMin = dZI + ( dDeltaZ / dPLen) * dl - dH ;
|
|
}
|
|
|
|
// Vettore per zona di punta inferiore
|
|
Vector3d vtCTI = ptC - ptPlTInfxy ;
|
|
// Proiezioni
|
|
double dPrTI1 = vtCTI * vtV1 ; double dPrTI2 = vtCTI * vtPlaneMoveOrt ;
|
|
|
|
// zona di punta inferiore
|
|
if ( dPrTI1 <= 0 && dPrTI1 > - dPLen1)
|
|
if ( dPrTI2 > - m_dRadius - dr - ( dPLen2 / dPLen1) * dPrTI1 && dPrTI2 < - ( dPLen2 / dPLen1) * dPrTI1) {
|
|
|
|
double dL = abs( - dr - ( dPLen2 / dPLen1) * dPrTI1 - dPrTI2) ;
|
|
double dH = ( m_dRadius * m_dRadius - dL * dL > 0 ? sqrt( m_dRadius * m_dRadius - dL * dL) : 0) ;
|
|
double dl = sqrt( dPrTI1 * dPrTI1 + ( ( dPLen2 / dPLen1) * dPrTI1) * ( ( dPLen2 / dPLen1) * dPrTI1)) ;
|
|
|
|
dMin = dZI + ( dDeltaZ / dPLen) * dl - dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingXYPlusBall( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
// Parte cilindrica
|
|
|
|
double dCylH = m_dHeight - m_dRadius ;
|
|
|
|
m_dHeight = dCylH ;
|
|
|
|
MillingXYPlusCyl( ptLs, ptLe, vtToolDir) ;
|
|
|
|
m_dHeight = m_dHeight + m_dRadius ;
|
|
////////////////////////////////////////
|
|
Point3d ptI, ptF ;
|
|
|
|
if ( ptLs.z < ptLe.z) {
|
|
|
|
ptI = ptLs ;
|
|
ptF = ptLe ;
|
|
}
|
|
else {
|
|
|
|
ptI = ptLe ;
|
|
ptF = ptLs ;
|
|
}
|
|
|
|
double dZI = ptI.z ; double dZF = ptF.z ; double dDeltaZ = dZF - dZI ;
|
|
|
|
Point3d ptCI = ptI - dCylH * vtToolDir ;
|
|
Point3d ptCF = ptF - dCylH * vtToolDir ;
|
|
|
|
Point3d ptCIxy( ptCI.x, ptCI.y, 0) ;
|
|
|
|
// Bounding box
|
|
double dMinX = min( ptCI.x, ptCF.x) - m_dRadius ;
|
|
double dMaxX = max( ptCI.x, ptCF.x) + m_dRadius ;
|
|
double dMinY = min( ptCI.y, ptCF.y) - m_dRadius ;
|
|
double dMaxY = max( ptCI.y, ptCF.y) + m_dRadius ;
|
|
|
|
Vector3d vtMove = ptF - ptI ;
|
|
Vector3d vtPlaneMove( vtMove.x, vtMove.y, 0) ; double dPLen = vtPlaneMove.LenXY() ; vtMove.Normalize() ;
|
|
|
|
// Sistema di riferimento nel piano
|
|
Vector3d vtV2 = vtPlaneMove ; vtV2.Normalize() ; double dComp1 = vtV2 * X_AX ; double dComp2 = vtV2 * Y_AX ;
|
|
Vector3d vtV1 = dComp2 * X_AX - dComp1 * Y_AX ;
|
|
|
|
// Determino il semi-asse minore
|
|
double dOriz = vtMove * Z_AX ; double dVert = vtMove * vtV2 ;
|
|
|
|
double dSemiAxMin = m_dRadius * dOriz ;
|
|
|
|
// Determinazione limiti sugli indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
// Ciclo
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dMin, dMax ;
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ;
|
|
|
|
Vector3d vtC = ptC - ptCIxy ;
|
|
|
|
double dProj1 = vtC * vtV1 ; double dProj2 = vtC * vtV2 ;
|
|
|
|
double dSqRadDistI = dProj1 * dProj1 + dProj2 * dProj2 ;
|
|
double dSqRadDistF = dProj1 * dProj1 + ( dProj2 - dPLen) * ( dProj2 - dPLen) ;
|
|
double dSqAxDist = dProj1 * dProj1 ;
|
|
|
|
if ( ( dProj2 < 0 && dSqRadDistI < m_dRadius * m_dRadius) ||
|
|
( dProj2 >= 0 && dProj2 < dPLen && dSqAxDist < m_dRadius * m_dRadius) ||
|
|
( dProj2 >= dPLen && dSqRadDistF < m_dRadius * m_dRadius)) {
|
|
|
|
// Massimi
|
|
if ( dProj2 < - dSemiAxMin * sqrt( 1 - ( dSqAxDist) / ( m_dRadius * m_dRadius))) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSqRadDistI) ;
|
|
|
|
dMax = dZI + dH ;
|
|
}
|
|
else if ( dProj2 >= - dSemiAxMin * sqrt( 1 - ( dSqAxDist) / ( m_dRadius * m_dRadius)) &&
|
|
dProj2 < dPLen - dSemiAxMin * sqrt( 1 - ( dSqAxDist) / ( m_dRadius * m_dRadius))) {
|
|
|
|
double dProj0 = - dSemiAxMin * sqrt( 1 - ( dSqAxDist) / ( m_dRadius * m_dRadius)) ;
|
|
double dZ0 = dZI + dVert * sqrt( m_dRadius * m_dRadius - dSqAxDist) ;
|
|
|
|
dMax = dZ0 + ( dDeltaZ / dPLen) * ( dProj2 - dProj0) ;
|
|
}
|
|
else {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSqRadDistF) ;
|
|
|
|
dMax = dZF + dH ;
|
|
}
|
|
|
|
// Minimi
|
|
if ( dProj2 < dSemiAxMin * sqrt( 1 - ( dSqAxDist) / ( m_dRadius * m_dRadius))) {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSqRadDistI) ;
|
|
|
|
dMin = dZI - dH ;
|
|
}
|
|
else if ( dProj2 >= dSemiAxMin * sqrt( 1 - ( dSqAxDist) / ( m_dRadius * m_dRadius)) &&
|
|
dProj2 < dPLen + dSemiAxMin * sqrt( 1 - ( dSqAxDist) / ( m_dRadius * m_dRadius))) {
|
|
|
|
double dProj0 = dSemiAxMin * sqrt( 1 - ( dSqAxDist) / ( m_dRadius * m_dRadius)) ;
|
|
double dZ0 = dZI - dVert * sqrt( m_dRadius * m_dRadius - dSqAxDist) ;
|
|
|
|
dMin = dZ0 + ( dDeltaZ / dPLen) * ( dProj2 - dProj0) ;
|
|
}
|
|
else {
|
|
|
|
double dH = sqrt( m_dRadius * m_dRadius - dSqRadDistF) ;
|
|
|
|
dMin = dZF - dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingXYPlusConus( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
double dMin, dMax ;
|
|
|
|
bool Control = BoundingBox( ptLs, ptLe, vtToolDir, vtToolDir) ;
|
|
|
|
if ( Control == false)
|
|
|
|
return true ;
|
|
|
|
double dMinRad = min( m_dRadius, m_dTipRadius) ;
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
double dDeltaR = dMaxRad - dMinRad ;
|
|
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
|
|
// Parte cilindrica
|
|
m_nToolType = CylindricalMill ;
|
|
m_dHeight = dCylH ;
|
|
|
|
MillingXYPlusCyl( ptLs, ptLe, vtToolDir) ;
|
|
|
|
m_nToolType = ConusMill ;
|
|
m_dHeight = m_dHeight + m_dTipHeight ;
|
|
|
|
// Variabili di interesse per la parte conica
|
|
Vector3d vtV1, vtV2, vtV3 ;
|
|
Point3d ptI, ptF ;
|
|
|
|
double dStem ;
|
|
|
|
// Studio della parte conica
|
|
if ( m_dRadius > m_dTipRadius) {
|
|
|
|
vtV1 = vtToolDir ;
|
|
dStem = - dCylH ;
|
|
}
|
|
else {
|
|
|
|
vtV1 = - vtToolDir ;
|
|
dStem = m_dHeight ;
|
|
}
|
|
|
|
ptI = ( ptLs.z < ptLe.z ? ptLs + vtV1 * dStem : ptLe + vtV1 * dStem) ;
|
|
ptF = ( ptLs.z < ptLe.z ? ptLe + vtV1 * dStem : ptLs + vtV1 * dStem) ;
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
|
|
Vector3d vtMove = ptF - ptI ;
|
|
Vector3d vtMoveLong = ( vtMove * vtV1) * vtV1 ;
|
|
Vector3d vtMoveOrt = vtMove - vtMoveLong ;
|
|
Vector3d vtMoveLongXY( vtMoveLong.x, vtMoveLong.y, 0) ;
|
|
Vector3d vtMoveOrtXY( vtMoveOrt.x, vtMoveOrt.y, 0) ;
|
|
|
|
double dPLen1 = vtMoveLongXY.LenXY() ;
|
|
double dPLen2 = vtMoveOrtXY.LenXY() ;
|
|
double dPLen = sqrt( dPLen1 * dPLen1 + dPLen2 * dPLen2) ;
|
|
|
|
vtV2 = ( vtMove * vtV1 > 0 ? vtMoveOrt : - vtMoveOrt) ; vtV2.Normalize() ;
|
|
vtV3 = vtV1 ^ vtV2 ;
|
|
|
|
Vector3d vtU2 = vtMoveOrtXY ; vtU2.Normalize() ;
|
|
|
|
Point3d ptV = ptI - vtV1 * ( ( dMaxRad * m_dTipHeight) / ( dDeltaR)) ;
|
|
|
|
// Apertura del cono e parametri per determinare i piani
|
|
double dTan = ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dRatio = ( vtMove * vtV1) / ( vtMove * vtV2) ;
|
|
|
|
double dCos = dTan * dRatio ; // dCos è compreso fra 0 e 1 poiché alpha è compreso fra 0 e Pi mezzi
|
|
double dSin = ( 1 - dCos * dCos > 0 ? sqrt( 1 - dCos * dCos) : 0) ;
|
|
|
|
double dDen = sqrt( 1 + dTan * dTan) ;
|
|
|
|
double dZI = ptI.z ;
|
|
double dDeltaZ = ptF.z - ptI.z ;
|
|
// double dCornerSlope = atan2( dDeltaZ, dPLen1) ; // dCornerSlope è compreso fra 0 ° e 90 °, per come è costruito il movimento, ma espresso in radianti
|
|
|
|
// Punti di tagenza piano cono
|
|
Point3d ptPrs = ptI - vtV1 * m_dTipHeight + dMinRad * ( dCos * vtV2 + sqrt( 1 - dCos * dCos) * vtV3) ;
|
|
Point3d ptPRs = ptI + dMaxRad * ( dCos * vtV2 + sqrt( 1 - dCos * dCos) * vtV3) ;
|
|
Point3d ptPrd = ptI - vtV1 * m_dTipHeight + dMinRad * ( dCos * vtV2 - sqrt( 1 - dCos * dCos) * vtV3) ;
|
|
Point3d ptPRd = ptI + dMaxRad * ( dCos * vtV2 - sqrt( 1 - dCos * dCos) * vtV3) ;
|
|
|
|
Point3d ptRInf = ( ptPRs.z < ptPRd.z ? ptPRs : ptPRd) ;
|
|
Point3d ptRSup = ( ptPRs.z < ptPRd.z ? ptPRd : ptPRs) ;
|
|
Point3d ptrInf = ( ptPrs.z < ptPrd.z ? ptPrs : ptPrd) ;
|
|
Point3d ptrSup = ( ptPrs.z < ptPrd.z ? ptPrd : ptPrs) ;
|
|
|
|
// Versori normali e prodotti scalari per per determinare i piani
|
|
Vector3d vtNs = - ( dTan / dDen) * vtV1 + ( dCos / dDen) * vtV2 + ( dSin / dDen) * vtV3 ;
|
|
Vector3d vtNd = - ( dTan / dDen) * vtV1 + ( dCos / dDen) * vtV2 - ( dSin / dDen) * vtV3 ;
|
|
|
|
Vector3d vtR0 = ptV - ORIG ;
|
|
|
|
double dDots = vtR0 * vtNs ;
|
|
double dDotd = vtR0 * vtNd ;
|
|
|
|
// Sistema di riferimento del movimento
|
|
Vector3d vtU3 = vtV1 ^ vtU2 ;
|
|
Frame3d MoveFrame ; MoveFrame.Set( ptI, vtV1, vtU2, vtU3) ;
|
|
|
|
ptRInf.LocToLoc( m_LocalFrame, MoveFrame) ; double dPRInf = ptRInf.y ;
|
|
ptRSup.LocToLoc( m_LocalFrame, MoveFrame) ; double dPRSup = ptRSup.y ;
|
|
ptrInf.LocToLoc( m_LocalFrame, MoveFrame) ; double dPrInf = ptrInf.y ;
|
|
ptrSup.LocToLoc( m_LocalFrame, MoveFrame) ; double dPrSup = ptrSup.y ;
|
|
|
|
// dMinX dMaxX dMinY dMaxY
|
|
double dMinX = min( min( ptI.x, ptF.x), min( ptI.x - vtV1.x * m_dTipHeight, ptF.x - vtV1.x * m_dTipHeight)) - dMaxRad;
|
|
double dMinY = min( min( ptI.y, ptF.y), min( ptI.y - vtV1.y * m_dTipHeight, ptF.y - vtV1.y * m_dTipHeight)) - dMaxRad;
|
|
double dMaxX = max( max( ptI.x, ptF.x), max( ptI.x - vtV1.x * m_dTipHeight, ptF.x - vtV1.x * m_dTipHeight)) + dMaxRad;
|
|
double dMaxY = max( max( ptI.y, ptF.y), max( ptI.y - vtV1.y * m_dTipHeight, ptF.y - vtV1.y * m_dTipHeight)) + dMaxRad;
|
|
|
|
// Verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return true ;
|
|
|
|
// Limiti su indici
|
|
unsigned int nStartI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
unsigned int nEndI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
unsigned int nStartJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
unsigned int nEndJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX, dY ; dX = ( i + 0.5) * m_dStep ; dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dI1 = vtC * vtV1 ; double dI2 = vtC * vtU2 ;
|
|
|
|
|
|
if ( dRatio <= 1 / dTan) {
|
|
|
|
|
|
if ( vtMove * vtV1 > 0) {
|
|
|
|
|
|
double dLimInf = max( - dMaxRad - dI1 * ( dMaxRad - dMinRad) / m_dTipHeight, - dMaxRad + dI1 * ( dPLen2 / dPLen1)) ;
|
|
double dLimSup = min( dMinRad + ( dI1 + m_dTipHeight) * ( dPLen2 / dPLen1), dPLen2 + dMinRad + ( dI1 - dPLen1 + m_dTipHeight) * ( dMaxRad - dMinRad) / m_dTipHeight) ;
|
|
|
|
if ( dI1 > - m_dTipHeight && dI1 < dPLen1 && dI2 > dLimInf && dI2 < dLimSup) {
|
|
|
|
// Massimi
|
|
if ( dI1 > 0 && dI2 < dPRSup + dI1 * ( dPLen2 / dPLen1)) {
|
|
|
|
double dr = dI1 * ( dPLen2 / dPLen1) - dI2 ; // Non serve prenderne il valore assoluto poiché viene elevato a quadrato
|
|
double dl = dI1 * sqrt( 1 + (dPLen2 * dPLen2) / ( dPLen1 * dPLen1)) ;
|
|
double dH = ( dMaxRad * dMaxRad - dr * dr > 0 ? sqrt( dMaxRad * dMaxRad - dr * dr) : 0) ;
|
|
|
|
dMax = dZI + dl * ( dDeltaZ / dPLen) + dH ;
|
|
}
|
|
else if ( dI1 < dPLen1 - m_dTipHeight && dI2 > dPrSup + ( dI1 + m_dTipHeight) * ( dPLen2 / dPLen1)) {
|
|
|
|
double dr = dI2 - ( dI1 + m_dTipHeight) * ( dPLen2 / dPLen1) ; // Non serve prenderne il valore assoluto poiché viene elevato a quadrato
|
|
double dl = ( dI1 + m_dTipHeight) * sqrt( 1 + ( dPLen2 * dPLen2) / ( dPLen1 * dPLen1)) ;
|
|
double dH = ( dMinRad * dMinRad - dr * dr > 0 ? sqrt( dMinRad * dMinRad - dr * dr) : 0) ;
|
|
// Controllare da qui
|
|
dMax = dZI + dl * ( dDeltaZ / dPLen) + dH ;
|
|
}
|
|
else if ( dI1 <= 0 && dI2 < dPRSup + dI1 * ( dPRSup - dPrSup) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad + dI1 * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - dI2 * dI2) ;
|
|
|
|
dMax = dZI + dH ;
|
|
}
|
|
else if ( dI1 >= dPLen1 - m_dTipHeight && dI2 > dPLen2 + dPRSup + ( dI1 - dPLen1) * ( dPRSup - dPrSup) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad + ( dI1 - dPLen1) * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - ( dI2 - dPLen2) * ( dI2 - dPLen2)) ;
|
|
|
|
dMax = dZI + dDeltaZ + dH ;
|
|
}
|
|
else
|
|
|
|
dMax = max( ( dDotd - vtNd.x * dX - vtNd.y * dY) / vtNd.z, ( dDots - vtNs.x * dX - vtNs.y * dY) / vtNs.z) ;
|
|
|
|
|
|
|
|
// Minimi
|
|
if ( dI1 > 0 && dI2 < dPRInf + dI1 * ( dPLen2 / dPLen1)) {
|
|
|
|
double dr = dI1 * ( dPLen2 / dPLen1) - dI2 ; // Non serve prenderne il valore assoluto poiché viene elevato a quadrato
|
|
double dl = dI1 * sqrt( 1 + ( dPLen2 * dPLen2) / ( dPLen1 * dPLen1)) ;
|
|
double dH = ( dMaxRad * dMaxRad - dr * dr > 0 ? sqrt( dMaxRad * dMaxRad - dr * dr) : 0) ;
|
|
|
|
dMin = dZI + dl * ( dDeltaZ / dPLen) - dH ;
|
|
}
|
|
else if ( dI1 < dPLen1 - m_dTipHeight && dI2 > dPrInf + ( dI1 + m_dTipHeight) * ( dPLen2 / dPLen1)) {
|
|
|
|
double dr = dI2 - ( dI1 + m_dTipHeight) * ( dPLen2 / dPLen1) ; // Non serve prenderne il valore assoluto poiché viene elevato a quadrato
|
|
double dl = ( dI1 + m_dTipHeight) * sqrt( 1 + ( dPLen2 * dPLen2) / ( dPLen1 * dPLen1)) ;
|
|
double dH = ( dMinRad * dMinRad - dr * dr > 0 ? sqrt( dMinRad * dMinRad - dr * dr) : 0) ;
|
|
|
|
dMin = dZI + dl * ( dDeltaZ / dPLen) - dH ;
|
|
}
|
|
else if ( dI1 <= 0 && dI2 < dPRInf + dI1 * ( dPRInf - dPrInf) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad + dI1 * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - dI2 * dI2) ;
|
|
|
|
dMin = dZI - dH ;
|
|
}
|
|
else if ( dI1 >= dPLen1 - m_dTipHeight && dI2 > dPLen2 + dPRInf + ( dI1 - dPLen1) * ( dPRInf - dPrInf) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad + ( dI1 - dPLen1) * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - ( dI2 - dPLen2) * ( dI2 - dPLen2)) ;
|
|
|
|
dMin = dZI + dDeltaZ - dH ;
|
|
}
|
|
else
|
|
|
|
dMin = min( ( dDotd - vtNd.x * dX - vtNd.y * dY) / vtNd.z, ( dDots - vtNs.x * dX - vtNs.y * dY) / vtNs.z) ;
|
|
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
// vtMove * vtV1 < 0
|
|
else {
|
|
|
|
double dLimInf = max( - dMaxRad - dI1 * ( dMaxRad - dMinRad) / m_dTipHeight, - dMinRad - ( dI1 + m_dTipHeight) * ( dPLen2 / dPLen1)) ;
|
|
double dLimSup = min( dMaxRad - dI1 * ( dPLen2 / dPLen1), dPLen2 + dMaxRad + ( dI1 + dPLen1) * ( dMaxRad - dMinRad) / m_dTipHeight) ;
|
|
|
|
if ( dI1 > - dPLen1 - m_dTipHeight && dI1 < 0 && dI2 > dLimInf && dI2 < dLimSup) {
|
|
|
|
// Massimi
|
|
if ( dI1 > - m_dTipHeight && dI2 < dPRSup + dI1 * ( dPRSup - dPrSup) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad + dI1 * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dH = ( dr * dr - dI2 * dI2 > 0 ? sqrt( dr * dr - dI2 * dI2) : 0) ;
|
|
|
|
dMax = dZI + dH ;
|
|
}
|
|
else if ( dI1 < - dPLen1 && dI2 > dPLen2 + dPRSup + (dI1 + dPLen1) * ( dPRSup - dPrSup) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad + ( dI1 + dPLen1) * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dH = ( dr * dr - ( dI2 - dPLen2) * ( dI2 - dPLen2) > 0 ? sqrt( dr * dr - ( dI2 - dPLen2) * ( dI2 - dPLen2)) : 0) ;
|
|
|
|
dMax = dZI + dDeltaZ + dH ;
|
|
}
|
|
else if ( dI1 < - m_dTipHeight && dI2 < dPrSup - ( dI1 + m_dTipHeight) * ( dPLen2 / dPLen1)) {
|
|
|
|
double dr = - ( dI1 + m_dTipHeight) * ( dPLen2 / dPLen1) - dI2 ;
|
|
double dH = ( dMinRad * dMinRad - dr * dr > 0 ? sqrt( dMinRad * dMinRad - dr * dr) : 0) ;
|
|
double dl = - ( dI1 + m_dTipHeight) * sqrt( 1 + ( dPLen2 * dPLen2) / ( dPLen1 * dPLen1)) ;
|
|
|
|
dMax = dZI + dl * ( dDeltaZ / dPLen) + dH ;
|
|
}
|
|
else if ( dI1 > - dPLen1 && dI2 > dPRSup - dI1 * ( dPLen2 / dPLen1)) {
|
|
|
|
double dr = dI2 + dI1 * ( dPLen2 / dPLen1) ;
|
|
double dH = ( dMaxRad * dMaxRad - dr * dr > 0 ? sqrt( dMaxRad * dMaxRad - dr * dr) : 0) ;
|
|
double dl = - dI1 * sqrt( 1 + ( dPLen2 * dPLen2) / ( dPLen1 * dPLen1)) ;
|
|
|
|
dMax = dZI + dl * ( dDeltaZ / dPLen) + dH ;
|
|
}
|
|
else
|
|
|
|
dMax = max( ( dDots - vtNs.x * dX - vtNs.y * dY) / vtNs.z, ( dDotd - vtNd.x * dX - vtNd.y * dY) / vtNd.z ) ;
|
|
|
|
|
|
// Minimi
|
|
if ( dI1 > - m_dTipHeight && dI2 < dPRInf + dI1 * ( dPRInf - dPrInf) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad + dI1 * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dH = ( dr * dr - dI2 * dI2 > 0 ? sqrt( dr * dr - dI2 * dI2) : 0) ;
|
|
|
|
dMin =dZI - dH ;
|
|
}
|
|
else if ( dI1 < - dPLen1 && dI2 > dPLen2 + dPRInf + ( dI1 + dPLen1) * ( dPRInf - dPrInf) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad + ( dI1 + dPLen1) * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dH = ( dr * dr - ( dI2 - dPLen2) * ( dI2 - dPLen2) > 0 ? sqrt( dr * dr - ( dI2 - dPLen2) * ( dI2 - dPLen2)) : 0) ;
|
|
|
|
dMin = dZI + dDeltaZ - dH ;
|
|
|
|
}
|
|
else if ( dI1 < - m_dTipHeight && dI2 < dPrInf - ( dI1 + m_dTipHeight) * ( dPLen2 / dPLen1)) {
|
|
|
|
double dr = - ( dI1 + m_dTipHeight) * ( dPLen2 / dPLen1) - dI2 ;
|
|
double dH = ( dMinRad * dMinRad - dr * dr > 0 ? sqrt( dMinRad * dMinRad - dr * dr) : 0) ;
|
|
double dl = - ( dI1 + m_dTipHeight) * sqrt( 1 + ( dPLen2 * dPLen2) / ( dPLen1 * dPLen1)) ;
|
|
|
|
dMin = dZI + dl * ( dDeltaZ / dPLen) - dH ;
|
|
}
|
|
else if ( dI1 > - dPLen1 && dI2 > dPRInf - dI1 * ( dPLen2 / dPLen1)) {
|
|
|
|
double dr = dI2 + dI1 * ( dPLen2 / dPLen1) ;
|
|
double dH = ( dMaxRad * dMaxRad - dr * dr > 0 ? sqrt( dMaxRad * dMaxRad - dr * dr) : 0) ;
|
|
double dl = - dI1 * sqrt( 1 + ( dPLen2 * dPLen2) / ( dPLen1 * dPLen1)) ;
|
|
|
|
dMin = dZI + dl * ( dDeltaZ / dPLen) - dH ;
|
|
}
|
|
else
|
|
|
|
dMin = min( ( dDotd - vtNd.x * dX - vtNd.y * dY) / vtNd.z, ( dDots - vtNs.x * dX - vtNs.y * dY) / vtNs.z) ;
|
|
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
// dRatio <= 1 / dTan
|
|
else {
|
|
|
|
if ( dI1 > - m_dTipHeight && dI1 < 0 &&
|
|
dI2 > - dMaxRad - dI1 * ( dMaxRad - dMinRad) / m_dTipHeight &&
|
|
dI2 < dMaxRad + dI1 * ( dMaxRad - dMinRad) / m_dTipHeight) {
|
|
|
|
double dr = dMaxRad + dI1 * ( dMaxRad - dMinRad) / m_dTipHeight ;
|
|
double dH = sqrt( dr * dr - dI2 * dI2) ;
|
|
|
|
dMin = dZI - dH ;
|
|
dMax = dZI + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( dI1 >= 0 && dI1 < dPLen1 &&
|
|
dI2 > - dMaxRad + dI1 * ( dPLen2 / dPLen1) &&
|
|
dI2 < dMaxRad + dI1 * ( dPLen2 / dPLen1)) {
|
|
|
|
double dl = dI1 * sqrt( 1 + ( dPLen2 * dPLen2) / ( dPLen1 * dPLen1)) ;
|
|
double dr = dI2 - dI1 * dPLen2 / dPLen1 ;
|
|
double dH = sqrt( dMaxRad * dMaxRad - dr * dr) ;
|
|
|
|
dMin = dZI + dl * ( dDeltaZ / dPLen) - dH ;
|
|
dMax = dZI + dl * ( dDeltaZ / dPLen) + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
// Virtual milling per componenti
|
|
|
|
// Versore utensile nella direzione dell'asse Z
|
|
|
|
// Foratura
|
|
//---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::DrillZ( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
// Posizioni iniziale e finale dell'utensile
|
|
Point3d ptI = ptLs ;
|
|
Point3d ptF = ptLe ;
|
|
|
|
// vettore movimento
|
|
Vector3d vtMove = ptLe - ptLs ;
|
|
|
|
CurveComposite ToolProfile ;
|
|
|
|
// Settaggio profilo
|
|
if ( m_ToolArcLineApprox.GetCurveCount() == 0)
|
|
// Se l'utensile non è stato approssimato uso l'originale
|
|
ToolProfile.CopyFrom( & m_ToolOutline) ;
|
|
else
|
|
// altrimenti usi l'approssimazione
|
|
ToolProfile.CopyFrom( & m_ToolArcLineApprox) ;
|
|
|
|
// Dichiaro un puntatore a curva da usare nel ciclo
|
|
const ICurve* pCurve ;
|
|
|
|
pCurve = ToolProfile.GetFirstCurve() ;
|
|
|
|
// Ciclo sulle curve
|
|
while ( pCurve != nullptr) {
|
|
|
|
double dHeight ;
|
|
|
|
int nCurveType = pCurve -> GetType() ;
|
|
|
|
// Caso segmento
|
|
if ( nCurveType == CRV_LINE) {
|
|
|
|
Point3d ptStart, ptEnd ;
|
|
|
|
pCurve -> GetStartPoint( ptStart) ;
|
|
pCurve -> GetEndPoint( ptEnd) ;
|
|
|
|
if ( abs( ptStart.y - ptEnd.y) > EPS_SMALL) {
|
|
|
|
dHeight = abs( ptStart.y - ptEnd.y) ;
|
|
|
|
// Il componente è un cilindro
|
|
if ( abs( ptStart.x - ptEnd.x) < EPS_SMALL) {
|
|
|
|
double dRadius = ptStart.x ;
|
|
|
|
LongCylV( ptI, ptF, vtToolDir, dHeight, dRadius) ;
|
|
}
|
|
// Il componente è un cono con vettore equiverso a quello dell'utensile
|
|
else if ( ptStart.x > ptEnd.x) {
|
|
|
|
double dMaxRad = ptStart.x ;
|
|
double dMinRad = ptEnd.x ;
|
|
|
|
LongConusV( ptI, ptF, vtToolDir, dHeight, dMaxRad, dMinRad) ;
|
|
}
|
|
// Il componente è un cono con vettore opposto a quello dell'utesile
|
|
else if ( ptStart.x < ptEnd.x) {
|
|
|
|
double dMaxRad = ptEnd.x ;
|
|
double dMinRad = ptStart.x ;
|
|
|
|
Point3d ptIn = ptI - vtToolDir * dHeight ;
|
|
Point3d ptFn = ptIn + vtMove ;
|
|
|
|
LongConusV( ptIn, ptFn, - vtToolDir, dHeight, dMaxRad, dMinRad) ;
|
|
}
|
|
}
|
|
else
|
|
dHeight = 0 ;
|
|
}
|
|
// Caso arco
|
|
else if ( nCurveType == CRV_ARC) {
|
|
|
|
// Centro e Punti iniziale e finale del cerchio
|
|
Point3d ptStart, ptEnd, ptO ;
|
|
|
|
pCurve -> GetStartPoint( ptStart) ;
|
|
pCurve -> GetEndPoint( ptEnd) ;
|
|
pCurve -> GetCenterPoint( ptO) ;
|
|
|
|
// Determino il raggio
|
|
Vector3d vtStRad = ptStart - ptO ;
|
|
Vector3d vtEnRad = ptEnd - ptO ;
|
|
|
|
double dRadius = 0.5 * ( vtStRad.LenXY() + vtEnRad.LenXY()) ;
|
|
|
|
// Determino le posizioni iniziale e finale del centrodella sfera
|
|
Point3d ptOSt = ptI - vtToolDir * ( ptStart.y - ptO.y) ;
|
|
Point3d ptOEn = ptOSt + vtMove ;
|
|
|
|
// Eseguo l'asportazione del materiale
|
|
Ball( ptOSt, ptOEn, dRadius) ;
|
|
|
|
|
|
// aggiorno l'altezza
|
|
dHeight = abs( ptStart.y - ptEnd.y) ;
|
|
}
|
|
|
|
// Determino le posizioni iniziale e finale del componente successivo
|
|
ptI = ptI - vtToolDir * dHeight ;
|
|
ptF = ptI + vtMove ;
|
|
|
|
// Aggiorno il puntatore
|
|
pCurve = ToolProfile.GetNextCurve() ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::LongCylV( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir, double dHei, double dRad) {
|
|
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BBoxComponent( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ, dRad, dRad, dHei) ;
|
|
|
|
if ( Control == false)
|
|
|
|
return true ;
|
|
|
|
Point3d ptI = ( ( ptLe - ptLs) * vtToolDir > 0 ? ptLe : ptLs) ;
|
|
Point3d ptF = ( ( ptLe - ptLs) * vtToolDir > 0 ? ptLs - dHei * vtToolDir : ptLe - dHei * vtToolDir) ;
|
|
|
|
if ( ptI.z > ptF.z) {
|
|
|
|
Point3d ptTemp = ptI ;
|
|
ptI = ptF ;
|
|
ptF = ptTemp ;
|
|
}
|
|
|
|
Point3d ptO( ptI.x, ptI.y, 0) ;
|
|
|
|
double dZI = ptI.z ;
|
|
double dZF = ptF.z ;
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i)
|
|
for( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptO ;
|
|
|
|
double dSqDist = vtC * vtC ;
|
|
|
|
if ( dSqDist < dRad * dRad)
|
|
|
|
SubtractIntervals( i, j, dZI, dZF) ;
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::LongConusV( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir, double dHei, double dMaxRad, double dMinRad) {
|
|
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BBoxComponent( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ, dMaxRad, dMinRad, dHei) ;
|
|
|
|
if ( Control == false)
|
|
|
|
return true ;
|
|
|
|
Point3d ptO( ptLs.x, ptLs.y, 0) ;
|
|
|
|
double dZMin, dZMax ;
|
|
|
|
double dAngC = dHei / ( dMaxRad - dMinRad) ;
|
|
|
|
if ( vtToolDir.z > 0) {
|
|
|
|
dZMin = ( ptLs.z < ptLe.z ? ptLs.z - dHei : ptLe.z - dHei) ;
|
|
dZMax = ( ptLs.z < ptLe.z ? ptLe.z : ptLs.z) ;
|
|
}
|
|
else {
|
|
|
|
dZMin = ( ptLs.z < ptLe.z ? ptLs.z : ptLe.z) ;
|
|
dZMax = ( ptLs.z < ptLe.z ? ptLe.z + dHei : ptLs.z + dHei) ;
|
|
}
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i)
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptO ;
|
|
|
|
double dSqDist = vtC * vtC ;
|
|
|
|
if ( dSqDist < dMinRad * dMinRad)
|
|
|
|
SubtractIntervals( i, j, dZMin, dZMax) ;
|
|
|
|
else if ( dSqDist < dMaxRad * dMaxRad) {
|
|
|
|
double dr = sqrt( dSqDist) ;
|
|
|
|
if ( vtToolDir.z > 0)
|
|
|
|
SubtractIntervals( i, j, dZMin + dAngC * ( dr - dMinRad), dZMax) ;
|
|
else
|
|
SubtractIntervals( i, j, dZMin, dZMax - dAngC * ( dr - dMinRad)) ;
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
// Fresatura
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillZ( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
// Posizioni iniziale e finale dell'utensile
|
|
Point3d ptI = ptLs ;
|
|
Point3d ptF = ptLe ;
|
|
|
|
// vettore movimento
|
|
Vector3d vtMove = ptLe - ptLs ;
|
|
|
|
CurveComposite ToolProfile ;
|
|
|
|
// Settaggio profilo
|
|
if ( m_ToolArcLineApprox.GetCurveCount() == 0)
|
|
// Se l'utensile non è stato approssimato uso l'originale
|
|
ToolProfile.CopyFrom( & m_ToolOutline) ;
|
|
else
|
|
// altrimenti usi l'approssimazione
|
|
ToolProfile.CopyFrom( & m_ToolArcLineApprox) ;
|
|
|
|
// Dichiaro un puntatore a curva da usare nel ciclo
|
|
const ICurve* pCurve ;
|
|
|
|
pCurve = ToolProfile.GetFirstCurve() ;
|
|
|
|
// Ciclo sulle curve
|
|
while ( pCurve != nullptr) {
|
|
|
|
double dHeight ;
|
|
|
|
int nCurveType = pCurve -> GetType() ;
|
|
|
|
// Caso segmento
|
|
if ( nCurveType == CRV_LINE) {
|
|
|
|
Point3d ptStart, ptEnd ;
|
|
|
|
pCurve -> GetStartPoint( ptStart) ;
|
|
pCurve -> GetEndPoint( ptEnd) ;
|
|
|
|
if ( abs( ptStart.y - ptEnd.y) > EPS_SMALL) {
|
|
|
|
dHeight = abs( ptStart.y - ptEnd.y) ;
|
|
|
|
// Il componente è un cilindro
|
|
if ( abs( ptStart.x - ptEnd.x) < EPS_SMALL) {
|
|
|
|
double dRadius = ptStart.x ;
|
|
|
|
MillCylV( ptI, ptF, vtToolDir, dHeight, dRadius) ;
|
|
}
|
|
// Il componente è un cono con vettore equiverso a quello dell'utensile
|
|
else if ( ptStart.x > ptEnd.x) {
|
|
|
|
double dMaxRad = ptStart.x ;
|
|
double dMinRad = ptEnd.x ;
|
|
|
|
MillConusV( ptI, ptF, vtToolDir, dHeight, dMaxRad, dMinRad) ;
|
|
}
|
|
// Il componente è un cono con vettore opposto a quello dell'utensile
|
|
else if ( ptStart.x < ptEnd.x) {
|
|
|
|
double dMaxRad = ptEnd.x ;
|
|
double dMinRad = ptStart.x ;
|
|
|
|
Point3d ptIn = ptI - vtToolDir * dHeight ;
|
|
Point3d ptFn = ptIn + vtMove ;
|
|
|
|
MillConusV( ptIn, ptFn, - vtToolDir, dHeight, dMaxRad, dMinRad) ;
|
|
}
|
|
}
|
|
else
|
|
dHeight = 0 ;
|
|
}
|
|
// Caso arco
|
|
else if ( nCurveType == CRV_ARC) {
|
|
|
|
// Centro e Punti iniziale e finale del cerchio
|
|
Point3d ptStart, ptEnd, ptO ;
|
|
|
|
pCurve -> GetStartPoint( ptStart) ;
|
|
pCurve -> GetEndPoint( ptEnd) ;
|
|
pCurve -> GetCenterPoint( ptO) ;
|
|
|
|
// Determino il raggio
|
|
Vector3d vtStRad = ptStart - ptO ;
|
|
Vector3d vtEnRad = ptEnd - ptO ;
|
|
|
|
double dRadius = 0.5 * ( vtStRad.LenXY() + vtEnRad.LenXY()) ;
|
|
|
|
// Determino le posizioni iniziale e finale del centrodella sfera
|
|
Point3d ptOSt = ptI - vtToolDir * ( ptStart.y - ptO.y) ;
|
|
Point3d ptOEn = ptOSt + vtMove ;
|
|
|
|
// Eseguo l'asportazione del materiale
|
|
Ball( ptOSt, ptOEn, dRadius) ;
|
|
|
|
// aggiorno l'altezza
|
|
dHeight = abs( ptStart.y - ptEnd.y) ;
|
|
}
|
|
|
|
// Determino le posizioni iniziale e finale del componente successivo
|
|
ptI = ptI - vtToolDir * dHeight ;
|
|
ptF = ptI + vtMove ;
|
|
|
|
// Aggiorno il puntatore
|
|
pCurve = ToolProfile.GetNextCurve() ;
|
|
}
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillCylV( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir, double dHei, double dRad)
|
|
{
|
|
double dMin, dMax ;
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BBoxComponent( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ, dRad, dRad, dHei) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
Point3d ptI, ptF ;
|
|
|
|
if ( ( ptLe - ptLs) * vtToolDir < 0 && vtToolDir.z > 0) {
|
|
|
|
ptI = ptLe ;
|
|
ptF = ptLs ;
|
|
}
|
|
else if ( ( ptLe - ptLs) * vtToolDir < 0 && vtToolDir.z < 0) {
|
|
|
|
ptI = ptLs - dHei * vtToolDir ;
|
|
ptF = ptLe - dHei * vtToolDir ;
|
|
}
|
|
else if ( ( ptLe - ptLs) * vtToolDir > 0 && vtToolDir.z < 0) {
|
|
|
|
ptI = ptLe - dHei * vtToolDir ;
|
|
ptF = ptLs - dHei * vtToolDir ;
|
|
}
|
|
else {
|
|
|
|
ptI = ( vtToolDir.z > 0 ? ptLs : ptLs - vtToolDir * dHei) ;
|
|
ptF = ( vtToolDir.z > 0 ? ptLe : ptLe - vtToolDir * dHei) ;
|
|
}
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
Point3d ptFxy( ptF.x, ptF.y, 0) ;
|
|
|
|
Vector3d vtV1 = ptFxy - ptIxy ; double dPLen = vtV1.LenXY() ; vtV1.Normalize() ;
|
|
Vector3d vtV2 = vtV1 ; vtV2.Rotate( Z_AX, 90) ;
|
|
|
|
double dZI = ptI.z ;
|
|
double dDeltaZ = ptF.z - ptI.z ;
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptIxy ; //Vector3d vtCF = ptC - ptFxy ;
|
|
|
|
double dX1 = vtC * vtV1 ; double dX2 = vtC * vtV2 ;
|
|
|
|
double dLimX1 = sqrt( dRad * dRad - dX2 * dX2) ;
|
|
|
|
if ( dX2 > - dRad && dX2 < dRad &&
|
|
dX1 > - dLimX1 &&
|
|
dX1 < dPLen + dLimX1) {
|
|
|
|
// Massimi
|
|
if( dX1 > - dLimX1 && dX1 < dPLen - dLimX1)
|
|
|
|
dMax = dZI + ( dX1 + dLimX1) * dDeltaZ / dPLen ;
|
|
|
|
else if ( dX1 >= dPLen - dLimX1 &&
|
|
dX1 < dPLen + dLimX1)
|
|
|
|
dMax = dZI + dDeltaZ ;
|
|
|
|
// Minimi
|
|
if ( dX1 > - dLimX1 && dX1 < dLimX1)
|
|
|
|
dMin = dZI - dHei ;
|
|
|
|
else if ( dX1 >= dLimX1 &&
|
|
dX1 < dPLen + dLimX1)
|
|
|
|
dMin = dZI - dHei + ( dX1 - dLimX1) * dDeltaZ / dPLen ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillConusV( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir,
|
|
double dHei, double dMaxRad, double dMinRad)
|
|
{
|
|
double dMin, dMax, dPLim, dMLim ;
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BBoxComponent( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ, dMaxRad, dMinRad, dHei) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
Point3d ptI = ( vtToolDir * ( ptLe - ptLs) > 0 ? ptLs : ptLe) ;
|
|
Point3d ptF = ( vtToolDir * ( ptLe - ptLs) > 0 ? ptLe : ptLs) ;
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
Point3d ptFxy( ptF.x, ptF.y, 0) ;
|
|
|
|
Vector3d vtMove = ptF - ptI ; double dLen = vtMove.Len() ;
|
|
Vector3d vtMLong = ( vtMove * vtToolDir) * vtToolDir ; double dLLong = vtMLong.Len() ;
|
|
Vector3d vtMOrt = vtMove - vtMLong ; double dLOrt = vtMOrt.Len() ;
|
|
|
|
Vector3d vtV1 = vtToolDir ;
|
|
Vector3d vtV2 = vtMOrt ; vtV2.Normalize() ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
|
|
double dZI = ptI.z ;
|
|
double dZTI = ptI.z - vtV1.z * dHei ;
|
|
double dDeltaZ = ptF.z - ptI.z ;
|
|
double dDeltaR = dMaxRad - dMinRad ;
|
|
|
|
double dTan = dDeltaR / dHei ;
|
|
double dRatio = ( vtMove * vtV1) / ( vtMove * vtV2) ;
|
|
|
|
double dCos = dTan * dRatio ;
|
|
double dSin = ( 1 - dCos * dCos > 0 ? sqrt( 1 - dCos * dCos) : 0) ;
|
|
|
|
double dDen = sqrt( 1 + dTan * dTan) ;
|
|
|
|
Point3d ptV = ptI - vtV1 * ( dHei * dMaxRad / dDeltaR) ;
|
|
|
|
Vector3d vtNs = - ( dTan / dDen) * vtV1 + ( dCos / dDen) * vtV2 + ( dSin / dDen) * vtV3 ;
|
|
Vector3d vtNd = - ( dTan / dDen) * vtV1 + ( dCos / dDen) * vtV2 - ( dSin / dDen) * vtV3 ;
|
|
Vector3d vtR0 = ptV - ORIG ;
|
|
|
|
double dDots = vtR0 * vtNs ;
|
|
double dDotd = vtR0 * vtNd ;
|
|
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ;
|
|
|
|
Vector3d vtCI = ptC - ptIxy ; double dSqDI = vtCI.SqLenXY() ;
|
|
Vector3d vtCF = ptC - ptFxy ; double dSqDF = vtCF.SqLenXY() ;
|
|
|
|
double dIDO = vtCI * vtV3 ;
|
|
double dIDL = vtCI * vtV2 ;
|
|
double dIVarCos = dIDL / sqrt( dSqDI) ;
|
|
|
|
double dFDL = vtCF * vtV2 ;
|
|
double dFVarCos = dFDL / sqrt( dSqDF) ;
|
|
|
|
if ( dSqDI < dMaxRad * dMaxRad || dSqDF < dMaxRad * dMaxRad ||
|
|
(abs( dIDO) < dMaxRad && dIDL > 0 && dIDL < dLOrt)) {
|
|
|
|
// Caso dTan > 1 / dRatio
|
|
if ( dRatio > 1 / dTan) {
|
|
|
|
// Limiti nella direzione positiva di vtV1
|
|
if ( dSqDF < dMaxRad * dMaxRad)
|
|
|
|
dPLim = dZI + dDeltaZ ;
|
|
|
|
else
|
|
|
|
dPLim = dZI + ( dIDL + sqrt( dMaxRad * dMaxRad - dIDO * dIDO)) * dDeltaZ / dLOrt ;
|
|
|
|
// Limiti nella direzione negativa di vtV1
|
|
if ( dSqDI < dMinRad * dMinRad)
|
|
|
|
dMLim = dZTI ;
|
|
|
|
else if ( dSqDI < dMaxRad * dMaxRad)
|
|
|
|
dMLim = dZTI + ( sqrt( dSqDI) - dMinRad) * ( dZI - dZTI) / dDeltaR ;
|
|
|
|
else
|
|
|
|
dMLim = dZI + ( dIDL - sqrt( dMaxRad * dMaxRad - dIDO * dIDO)) * dDeltaZ / dLOrt ;
|
|
|
|
}
|
|
else {
|
|
|
|
// Limiti nella direzione positiva di vtV1
|
|
if ( dSqDF < dMaxRad * dMaxRad)
|
|
|
|
dPLim = dZI + dDeltaZ ;
|
|
|
|
else
|
|
|
|
dPLim = dZI + ( dIDL + sqrt( dMaxRad * dMaxRad - dIDO * dIDO)) * dDeltaZ / dLOrt ;
|
|
|
|
// Limiti nella direzione negativa di vtV1
|
|
if ( dSqDI < dMinRad * dMinRad)
|
|
|
|
dMLim = dZTI ;
|
|
|
|
else if ( dSqDI >= dMinRad * dMinRad && dSqDI < dMaxRad * dMaxRad && dIVarCos < dCos)
|
|
|
|
dMLim = dZTI + ( sqrt( dSqDI) - dMinRad) * ( dZI - dZTI) / dDeltaR ;
|
|
|
|
else if ( dSqDI >= dMinRad * dMinRad && dIVarCos >= dCos && dFVarCos < dCos && abs( dIDO) < dMaxRad * dSin) { // da qui
|
|
|
|
if ( dIDO > - dMaxRad * dSin && dIDO <= - dMinRad * dSin)
|
|
|
|
dMLim = ( dDotd - dX * vtNd.x - dY * vtNd.y) / vtNd.z ;
|
|
|
|
else if ( dIDO > - dMinRad * dSin && dIDO < dMinRad * dSin)
|
|
|
|
dMLim = dZTI + ( dIDL - sqrt( dMinRad * dMinRad - dIDO * dIDO)) * dDeltaZ / dLOrt ;
|
|
|
|
else if ( dIDO >= dMinRad * dSin && dIDO < dMaxRad * dSin)
|
|
|
|
dMLim = ( dDots - dX * vtNs.x - dY * vtNs.y) / vtNs.z ; // a qui
|
|
}
|
|
else if ( dFVarCos >= dCos) {
|
|
|
|
if ( dSqDF < dMinRad * dMinRad)
|
|
|
|
dMLim = dZTI + ( dIDL - sqrt( dMinRad * dMinRad - dIDO * dIDO)) * dDeltaZ / dLOrt ;
|
|
|
|
else
|
|
|
|
dMLim = dZTI + dDeltaZ + ( sqrt( dSqDF) - dMinRad) * ( dZI - dZTI) / dDeltaR ;
|
|
}
|
|
else
|
|
|
|
dMLim = dZI + ( dIDL - sqrt( dMaxRad * dMaxRad - dIDO * dIDO)) * dDeltaZ / dLOrt ;
|
|
}
|
|
|
|
dMin = min( dPLim, dMLim) ;
|
|
dMax = max( dPLim, dMLim) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
// Direzione generica del versore utensile
|
|
// Foratura
|
|
//----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::Drilling( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
if ( m_nToolType == CylindricalMill)
|
|
|
|
LongCyl( ptLs, ptLe, vtToolDir, m_dHeight, m_dRadius) ;
|
|
|
|
else if ( m_nToolType == BallEndMill) {
|
|
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
|
|
LongCyl( ptLs, ptLe, vtToolDir, dCylH, m_dRadius) ;
|
|
|
|
Point3d ptBs = ptLs - vtToolDir * ( m_dHeight - m_dTipHeight) ;
|
|
Point3d ptBe = ptLe - vtToolDir * ( m_dHeight - m_dTipHeight) ;
|
|
|
|
Ball( ptBs, ptBe, m_dRadius) ;
|
|
}
|
|
|
|
else if ( m_nToolType == BullNoseMill)
|
|
// Caso al momento non gestito
|
|
return false ;
|
|
|
|
else if ( m_nToolType == ConusMill) {
|
|
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
|
|
LongCyl( ptLs, ptLe, vtToolDir, dCylH, m_dRadius) ;
|
|
|
|
double dMinRad = ( m_dRadius > m_dTipRadius ? m_dTipRadius : m_dRadius) ;
|
|
double dMaxRad = ( m_dRadius > m_dTipRadius ? m_dRadius : m_dTipRadius) ;
|
|
|
|
Point3d ptCs = ( m_dRadius > m_dTipRadius ? ptLs - dCylH * vtToolDir : ptLs - m_dHeight * vtToolDir) ;
|
|
Point3d ptCe = ( m_dRadius > m_dTipRadius ? ptLe - dCylH * vtToolDir : ptLe - m_dHeight * vtToolDir) ;
|
|
Vector3d vtDir = ( m_dRadius > m_dTipRadius ? vtToolDir : - vtToolDir) ;
|
|
|
|
LongConus( ptCs, ptCe, vtDir, m_dTipHeight, dMaxRad, dMinRad) ;
|
|
}
|
|
|
|
else if ( m_nToolType == GenericTool)
|
|
// Caso al momento non gestito
|
|
return false ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::DrillingGT( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
// Posizioni iniziale e finale dell'utensile
|
|
Point3d ptI = ptLs ;
|
|
Point3d ptF = ptLe ;
|
|
|
|
// vettore movimento
|
|
Vector3d vtMove = ptLe - ptLs ;
|
|
|
|
CurveComposite ToolProfile ;
|
|
|
|
// Settaggio profilo
|
|
if ( m_ToolArcLineApprox.GetCurveCount() == 0)
|
|
// Se l'utensile non è stato approssimato uso l'originale
|
|
ToolProfile.CopyFrom( & m_ToolOutline) ;
|
|
else
|
|
// altrimenti usi l'approssimazione
|
|
ToolProfile.CopyFrom( & m_ToolArcLineApprox) ;
|
|
|
|
// Dichiaro un puntatore a curva da usare nel ciclo
|
|
const ICurve* pCurve ;
|
|
|
|
pCurve = ToolProfile.GetFirstCurve() ;
|
|
|
|
// Ciclo sulle curve
|
|
while ( pCurve != nullptr) {
|
|
|
|
double dHeight ;
|
|
|
|
int nCurveType = pCurve -> GetType() ;
|
|
|
|
// Caso di semento
|
|
if ( nCurveType == CRV_LINE) {
|
|
|
|
Point3d ptStart, ptEnd ;
|
|
|
|
pCurve -> GetStartPoint( ptStart) ;
|
|
pCurve -> GetEndPoint( ptEnd) ;
|
|
|
|
if ( abs( ptStart.y - ptEnd.y) > EPS_SMALL) {
|
|
|
|
dHeight = abs( ptStart.y - ptEnd.y) ;
|
|
|
|
// Il componente è un cilindro
|
|
if ( abs( ptStart.x - ptEnd.x) < EPS_SMALL) {
|
|
|
|
double dRadius = ptStart.x ;
|
|
|
|
LongCyl( ptI, ptF, vtToolDir, dHeight, dRadius) ;
|
|
}
|
|
// Il componente è un cono con vettore equiverso a quello dell'utensile
|
|
else if ( ptStart.x > ptEnd.x) {
|
|
|
|
double dMaxRad = ptStart.x ;
|
|
double dMinRad = ptEnd.x ;
|
|
|
|
LongConus( ptI, ptF, vtToolDir, dHeight, dMaxRad, dMinRad) ;
|
|
}
|
|
// Il componente è un cono con vettore opposto a quello dell'utensile
|
|
else if ( ptStart.x < ptEnd.x) {
|
|
|
|
double dMaxRad = ptEnd.x ;
|
|
double dMinRad = ptStart.x ;
|
|
|
|
Point3d ptIn = ptI - vtToolDir * dHeight ;
|
|
Point3d ptFn = ptIn + vtMove ;
|
|
|
|
LongConus( ptIn, ptFn, - vtToolDir, dHeight, dMaxRad, dMinRad) ;
|
|
}
|
|
}
|
|
else
|
|
dHeight = 0 ;
|
|
}
|
|
|
|
// Caso arco
|
|
else if ( nCurveType == CRV_ARC) {
|
|
|
|
// Centro e Punti iniziale e finale del cerchio
|
|
Point3d ptStart, ptEnd, ptO ;
|
|
|
|
pCurve -> GetStartPoint( ptStart) ;
|
|
pCurve -> GetEndPoint( ptEnd) ;
|
|
pCurve -> GetCenterPoint( ptO) ;
|
|
|
|
// Determino il raggio
|
|
Vector3d vtStRad = ptStart - ptO ;
|
|
Vector3d vtEnRad = ptEnd - ptO ;
|
|
|
|
double dRadius = 0.5 * ( vtStRad.LenXY() + vtEnRad.LenXY()) ;
|
|
|
|
// Determino le posizioni iniziale e finale del centrodella sfera
|
|
Point3d ptOSt = ptI - vtToolDir * ( ptStart.y - ptO.y) ;
|
|
Point3d ptOEn = ptOSt + vtMove ;
|
|
|
|
// Eseguo l'asportazione del materiale
|
|
Ball( ptOSt, ptOEn, dRadius) ;
|
|
|
|
|
|
// aggiorno l'altezza
|
|
dHeight = abs( ptStart.y - ptEnd.y) ;
|
|
}
|
|
|
|
// Determino le posizioni iniziale e finale del componente successivo
|
|
ptI = ptI - vtToolDir * dHeight ;
|
|
ptF = ptI + vtMove ;
|
|
|
|
// Aggiorno il puntatore
|
|
pCurve = ToolProfile.GetNextCurve() ;
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
// Componenti elementari degli utensili
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::LongCyl( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir, double dHei, double dRad)
|
|
{
|
|
double dMin, dMax;
|
|
unsigned int nStartI, nEndI, nStartJ, nEndJ ;
|
|
|
|
bool Control = BBoxComponent( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ, dRad, dRad, dHei) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
// Studio delle simmetrie
|
|
Vector3d vtMove = ptLe - ptLs ;
|
|
|
|
Point3d ptI = ( vtMove * vtToolDir > 0 ? ptLe : ptLs) ;
|
|
Point3d ptF = ( vtMove * vtToolDir > 0 ? ptLs - dHei * vtToolDir : ptLe - dHei * vtToolDir) ;
|
|
|
|
if ( ptI.z > ptF.z) {
|
|
|
|
Point3d ptTemp = ptI ;
|
|
ptI = ptF ;
|
|
ptF = ptTemp ;
|
|
}
|
|
|
|
double dDeltaZ = ptF.z - ptI.z ;
|
|
double dZI = ptI.z ;
|
|
|
|
// Definizione
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
Point3d ptFxy( ptF.x, ptF.y, 0) ;
|
|
|
|
Vector3d vtCyl = ptF - ptI ; double dLen = sqrt( vtCyl * vtCyl) ;
|
|
Vector3d vtCylVer( 0, 0, vtCyl.z) ; double dLVer = abs( vtCyl.z) ;
|
|
Vector3d vtCylOri( vtCyl.x, vtCyl.y, 0) ; double dLOri = vtCylOri.LenXY() ;
|
|
|
|
double dCos = dLVer / dLen ; // Coseno dell'angolo formato da vtCyl con l'asse Z.
|
|
double dSin = dLOri / dLen ; // Seno dell'angolo formato da vtCyl con l'asse Z.
|
|
|
|
double dSemiMin = dRad * dCos ;
|
|
|
|
// Definizione del sistema di riferimento nel piano
|
|
Vector3d vtU1 = vtCylOri ;
|
|
|
|
if ( vtU1.LenXY() < EPS_SMALL) {
|
|
|
|
double dLenVector = sqrt(vtCyl.x * vtCyl.x + vtCyl.y * vtCyl.y) ;
|
|
|
|
vtU1 = ( 1 + dLenVector) / dLenVector * vtU1 ;
|
|
}
|
|
|
|
vtU1.Normalize() ;
|
|
|
|
Vector3d vtU2 = vtU1 ; vtU2.Rotate( Z_AX, 90) ;
|
|
|
|
// Definizione piani
|
|
Vector3d vtV = vtMove ; vtV.Normalize() ;
|
|
Vector3d vtRI = ptI - ORIG ; double dDotI = vtRI * vtV ;
|
|
Vector3d vtRF = ptF - ORIG ; double dDotF = vtRF * vtV ;
|
|
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtCI = ptC - ptIxy ; Vector3d vtCF = ptC - ptFxy ;
|
|
|
|
double dProjI1 = vtCI * vtU1 ; double dProjI2 = vtCI * vtU2 ;
|
|
double dProjF1 = vtCF * vtU1 ; double dProjF2 = vtCF * vtU2 ;
|
|
|
|
if ( dProjI1 > - dCos * sqrt( dRad * dRad - dProjI2 * dProjI2) &&
|
|
dProjI1 < dLOri + dCos * sqrt( dRad * dRad - dProjI2 * dProjI2) &&
|
|
dProjI2 * dProjI2 < dRad * dRad) {
|
|
|
|
// Massimi
|
|
if ( dProjI1 < dLOri - dCos * sqrt( dRad * dRad - dProjI2 * dProjI2)) {
|
|
|
|
double dZ0 = dSin * sqrt( dRad * dRad - dProjI2 * dProjI2) ;
|
|
double dI10 = - dCos * sqrt( dRad * dRad - dProjI2 * dProjI2) ;
|
|
|
|
dMax = dZI + dZ0 + ( dProjI1 - dI10) * dDeltaZ / dLOri ;
|
|
}
|
|
else
|
|
|
|
dMax = ( dDotF - vtV.x * dX - vtV.y *dY) / vtV.z ;
|
|
|
|
// Minimi
|
|
if ( dProjI1 < dCos * sqrt( dRad * dRad - dProjI2 * dProjI2))
|
|
|
|
dMin = ( dDotI - vtV.x * dX - vtV.y *dY) / vtV.z ;
|
|
|
|
else {
|
|
|
|
double dZ0 = - dSin * sqrt( dRad * dRad - dProjI2 * dProjI2) ;
|
|
double dI10 = dCos * sqrt( dRad * dRad - dProjI2 * dProjI2) ;
|
|
|
|
dMin = dZI + dZ0 + ( dProjI1 - dI10) * dDeltaZ / dLOri ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::LongBall( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir, double dRad)
|
|
{
|
|
double dMin, dMax ;
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BBoxComponent( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ, dRad, dRad, 0) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
Point3d ptI = ( ( ptLe - ptLs) * vtToolDir < 0 ? ptLs : ptLe) ;
|
|
Point3d ptF = ( ( ptLe - ptLs) * vtToolDir < 0 ? ptLe : ptLs) ;
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
Point3d ptFxy( ptF.x, ptF.y, 0) ;
|
|
|
|
double dDeltaZ = ptF.z - ptI.z ;
|
|
double dZI = ptI.z ;
|
|
|
|
Vector3d vtMove = ptF - ptI ; double dLen = vtMove.Len() ;
|
|
Vector3d vtMoveVer( 0, 0, vtMove.z) ; double dLVer = abs( vtMove.z) ;
|
|
Vector3d vtMoveOri( vtMove.x, vtMove.y, 0) ; double dLOri = vtMoveOri.LenXY() ;
|
|
|
|
// Definizione del sistema di riferimento nel piano
|
|
Vector3d vtU1 = vtMoveOri ;
|
|
|
|
if ( vtU1.LenXY() < EPS_SMALL) {
|
|
|
|
double dLenVector = sqrt(vtMove.x * vtMove.x + vtMove.y * vtMove.y) ;
|
|
|
|
vtU1 = ( 1 + dLenVector) / dLenVector * vtU1 ;
|
|
}
|
|
|
|
|
|
vtU1.Normalize() ;
|
|
|
|
Vector3d vtU2 = vtU1 ; vtU2.Rotate( Z_AX, 90) ;
|
|
|
|
double dCos = dLVer / dLen ; // Sempre positivo
|
|
double dSin = dLOri / dLen ; // Sempre positivo
|
|
|
|
double dSemiMin = dRad * abs( dCos) ;
|
|
|
|
Vector3d vtV = vtMove ; vtV.Normalize() ;
|
|
|
|
Vector3d vtRI = ptI - ORIG ; double dDotI = vtRI * vtV ;
|
|
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ;
|
|
|
|
Vector3d vtCI = ptC - ptIxy ; Vector3d vtCF = ptC - ptFxy ;
|
|
|
|
double dPI1 = vtCI * vtU1 ; double dPI2 = vtCI * vtU2 ;
|
|
double dPF1 = vtCF * vtU1 ; double dPF2 = vtCF * vtU2 ;
|
|
|
|
double dLimP1 = sqrt( dRad * dRad - dPI2 * dPI2) ;
|
|
|
|
// Dexel nella regione interessata dalla lavorazione
|
|
if ( ( dPI1 > - dCos * dLimP1 &&
|
|
dPI1 < dLOri && dPI2 * dPI2 < dRad * dRad) ||
|
|
( dPF1 * dPF1 + dPF2 * dPF2 < dRad * dRad)) {
|
|
|
|
if ( dDeltaZ > 0) {
|
|
|
|
// Massimi
|
|
if ( dPI1 < dLOri - dCos * dLimP1) {
|
|
|
|
double dPI0 = - dCos * dLimP1 ;
|
|
double dZ0 = dSin * dLimP1 ;
|
|
|
|
dMax = dZI + dZ0 + ( dPI1 - dPI0) * dDeltaZ / dLOri ;
|
|
}
|
|
else {
|
|
|
|
double dSqD = dPF1 * dPF1 + dPF2 * dPF2 ;
|
|
double dH = sqrt( dRad * dRad - dSqD) ;
|
|
|
|
dMax = dZI + dDeltaZ + dH ;
|
|
}
|
|
|
|
// Minimi
|
|
if ( dPI1 < dCos * dLimP1)
|
|
|
|
dMin = ( dDotI - dX * vtV.x - dY * vtV.y) / vtV.z ;
|
|
|
|
else if ( dPI1 < dLOri + dCos * dLimP1) {
|
|
|
|
double dPI0 = dCos * dLimP1 ;
|
|
double dZ0 = - dSin * dLimP1 ;
|
|
|
|
dMin = dZI + dZ0 + ( dPI1 - dPI0) * dDeltaZ / dLOri ;
|
|
}
|
|
else {
|
|
|
|
double dSqD = dPF1 * dPF1 + dPF2 * dPF2 ;
|
|
double dH = sqrt( dRad * dRad - dSqD) ;
|
|
|
|
dMin = dZI + dDeltaZ - dH ;
|
|
}
|
|
}
|
|
else {
|
|
|
|
// Massimi
|
|
if ( dPI1 < dCos * dLimP1)
|
|
|
|
dMax = ( dDotI - dX * vtV.x - dY * vtV.y) / vtV.z ;
|
|
|
|
else if ( dPI1 < dLOri + dCos * dLimP1) {
|
|
|
|
double dPI0 = dCos * dLimP1 ;
|
|
double dZ0 = + dSin * dLimP1 ;
|
|
|
|
dMax = dZI + dZ0 + ( dPI1 - dPI0) * dDeltaZ / dLOri ;
|
|
}
|
|
else {
|
|
|
|
double dSqD = dPF1 * dPF1 + dPF2 * dPF2 ;
|
|
double dH = sqrt( dRad * dRad - dSqD) ;
|
|
|
|
dMax = dZI + dDeltaZ + dH ;
|
|
}
|
|
|
|
// Minimi
|
|
if ( dPI1 < dLOri - dCos * dLimP1) {
|
|
|
|
double dPI0 = - dCos * dLimP1 ;
|
|
double dZ0 = - dSin * dLimP1 ;
|
|
|
|
dMin = dZI + dZ0 + ( dPI1 - dPI0) * dDeltaZ / dLOri ;
|
|
}
|
|
else {
|
|
|
|
double dSqD = dPF1 * dPF1 + dPF2 * dPF2 ;
|
|
double dH = sqrt( dRad * dRad - dSqD) ;
|
|
|
|
dMin = dZI + dDeltaZ - dH ;
|
|
}
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::LongConus( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir,
|
|
double dHei, double dMaxRad, double dMinRad)
|
|
{
|
|
double dMin, dMax ;
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BBoxComponent( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ, dMaxRad, dMinRad, dHei) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
Vector3d vtMove = ptLe - ptLs ; double dLen = vtMove.Len() ;
|
|
Vector3d vtMZ( 0, 0, vtMove.z) ; double dLVer = abs( vtMove.z) ;
|
|
Vector3d vtMXY( vtMove.x, vtMove.y, 0) ; double dLOri = vtMXY.LenXY() ;
|
|
|
|
double dSin = dLOri / dLen ;
|
|
double dCos = dLVer / dLen ;
|
|
|
|
double dSemiMinR = dMaxRad * dCos ;
|
|
double dSemiMinr = dMinRad * dCos ;
|
|
|
|
// Sistema di riferimento sul cono
|
|
Vector3d vtV1 = vtToolDir ; // controllare qui e negli altri coni che le proiezioni non siano troppo piccole FORSE CONVIENE FARE I CONTI CON VTMOVE NORMALIZZATO (QUESTO IN TUTTI I MOVIMENTI)
|
|
|
|
double dCoef23 = ( vtV1.z > 0 ? 1 : - 1) ;
|
|
double dCoef21 = - dCoef23 * vtV1.z ; // vtV1.z := vtV1 * Z_AX
|
|
|
|
Vector3d vtV2 = dCoef21 * vtV1 + dCoef23 * Z_AX ; vtV2.Normalize() ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
|
|
// Simmetrie del problema riguardanti il cono
|
|
Point3d ptCBot = ( vtV1 * vtMove > 0 ? ptLs : ptLe) ;
|
|
Point3d ptCTip = ptCBot - dHei * vtV1 ;
|
|
|
|
double dDeltaR = dMaxRad - dMinRad ;
|
|
double dTan = dDeltaR / dHei ;
|
|
double dL = ( ( dMaxRad * dHei) / dDeltaR) ;
|
|
double dl = dL - dHei ;
|
|
|
|
Point3d ptV = ptCBot - vtV1 * dL ;
|
|
|
|
// Simmetrie del problema riguardanti il cilinidro
|
|
Point3d ptCylI = ( ptLs.z < ptLe.z ? ptLs : ptLe) ;
|
|
Point3d ptCylF = ( ptLs.z < ptLe.z ? ptLe : ptLs) ;
|
|
|
|
double dDeltaZ = ptCylF.z - ptCylI.z ;
|
|
double dZCylI = ptCylI.z ;
|
|
|
|
// Piani cono
|
|
Vector3d vtR0B = ptCBot - ORIG ; double dDotB = vtR0B * vtV1 ;
|
|
Vector3d vtR0T = ptCTip - ORIG ; double dDotT = vtR0T * vtV1 ;
|
|
|
|
|
|
// Piani cilindro
|
|
Vector3d vtR0I = ptCylI - ORIG ; double dDotI = vtR0I * vtV1 ;
|
|
Vector3d vtR0F = ptCylF - ORIG ; double dDotF = vtR0F * vtV1 ;
|
|
|
|
|
|
// Punti sul piano
|
|
Point3d ptCylIxy( ptCylI.x, ptCylI.y, 0) ;
|
|
Point3d ptCBotxy( ptCBot.x, ptCBot.y, 0) ;
|
|
|
|
|
|
// Riferimenti sul piano
|
|
Vector3d vtU1( ptCylF.x - ptCylI.x, ptCylF.y - ptCylI.y, 0) ; vtU1.Normalize() ;
|
|
Vector3d vtU2 = vtU1 ; vtU2.Rotate( Z_AX, 90) ;
|
|
|
|
Vector3d vtW1( vtV1.x, vtV1.y, 0) ; vtW1.Normalize() ;
|
|
Vector3d vtW2 = vtW1 ; vtW2.Rotate( Z_AX, 90) ;
|
|
|
|
// Sistema di riferimento del cono
|
|
Frame3d ConusFrame ; ConusFrame.Set( ptV, vtV1, vtV2, vtV3) ;
|
|
|
|
|
|
// Ciclo
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptCylIxy ;
|
|
|
|
double dPCyl1 = vtC * vtU1 ; double dPCyl2 = vtC * vtU2 ;
|
|
|
|
// Parte cilindrica
|
|
if ( dPCyl2 * dPCyl2 < dMaxRad * dMaxRad &&
|
|
dPCyl1 > - dSemiMinR * sqrt( 1 - ( dPCyl2 * dPCyl2) / ( dMaxRad * dMaxRad)) &&
|
|
dPCyl1 < dLOri + dSemiMinR * sqrt( 1 - ( dPCyl2 * dPCyl2) / ( dMaxRad * dMaxRad))) {
|
|
|
|
// Massimi
|
|
if ( dPCyl1 < dLOri - dSemiMinR * sqrt( 1 - ( dPCyl2 * dPCyl2) / ( dMaxRad * dMaxRad))) {
|
|
|
|
double dZ0 = dSin * sqrt( dMaxRad * dMaxRad - dPCyl2 * dPCyl2) ;
|
|
double dP0 = - dSemiMinR * sqrt( 1 - ( dPCyl2 * dPCyl2) / ( dMaxRad * dMaxRad)) ;
|
|
|
|
dMax = dZCylI + dZ0 + ( dPCyl1 - dP0) * dDeltaZ / ( dLOri) ;
|
|
}
|
|
else
|
|
|
|
dMax = ( dDotF - dX * vtV1.x - dY * vtV1.y) / vtV1.z ;
|
|
|
|
// Minimi
|
|
if ( dPCyl1 < dSemiMinR * sqrt( 1 - ( dPCyl2 * dPCyl2) / ( dMaxRad * dMaxRad)))
|
|
|
|
dMin = ( dDotI - dX * vtV1.x - dY * vtV1.y) / vtV1.z ;
|
|
|
|
else {
|
|
|
|
double dZ0 = - dSin * sqrt( dMaxRad * dMaxRad - dPCyl2 * dPCyl2) ;
|
|
double dP0 = dSemiMinR * sqrt( 1 - ( dPCyl2 * dPCyl2) / ( dMaxRad * dMaxRad)) ;
|
|
|
|
dMin = dZCylI + dZ0 + ( dPCyl1 - dP0) * dDeltaZ / ( dLOri) ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
// Parte conica
|
|
Vector3d vtD = Z_AX ;
|
|
|
|
ptC.LocToLoc( m_LocalFrame, ConusFrame) ;
|
|
|
|
vtD.LocToLoc( m_LocalFrame, ConusFrame) ;
|
|
|
|
std::vector <double> vdCoef(3);
|
|
std::vector <double> vdRoots;
|
|
|
|
vdCoef[0] = ( dTan * dTan * ptC.x * ptC.x - ptC.y * ptC.y - ptC.z * ptC.z) ;
|
|
vdCoef[1] = 2 * ( dTan * dTan * ptC.x * vtD.x - ptC.y * vtD.y - ptC.z * vtD.z) ;
|
|
vdCoef[2] = dTan * dTan * vtD.x * vtD.x - vtD.y * vtD.y - vtD.z * vtD.z ;
|
|
|
|
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
|
|
|
|
if ( nRoot == 1) {
|
|
|
|
Point3d ptR1 = ptC + vdRoots[0] * vtD ;
|
|
|
|
if ( ptR1.x >= dl && ptR1.x < dL) {
|
|
|
|
ptR1.LocToLoc( ConusFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR1.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= 0 && ptR1.x < dl) {
|
|
|
|
dMin = min( ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( nRoot == 2) {
|
|
|
|
Point3d ptR1 = ptC + vdRoots[0] * vtD ;
|
|
Point3d ptR2 = ptC + vdRoots[1] * vtD ;
|
|
|
|
if ( ptR1.x > ptR2.x) {
|
|
|
|
Point3d ptTemp = ptR1 ;
|
|
ptR1 = ptR2 ;
|
|
ptR2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptR1.x < 0 && ptR2.x > 0 && ptR2.x < dl) {
|
|
|
|
dMin = min( ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x < 0 && ptR2.x >= dl && ptR2.x < dL) {
|
|
|
|
ptR2.LocToLoc( ConusFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR2.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR2.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x > 0 && ptR1.x < dl && ptR2.x >= dl && ptR2.x < dL) {
|
|
|
|
ptR2.LocToLoc( ConusFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR2.z, ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR2.z, ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x > 0 && ptR1.x < dl && ptR2.x >= dL) {
|
|
|
|
dMin = min( ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= dl && ptR1.x < dL && ptR2.x < dL) {
|
|
|
|
ptR1.LocToLoc( ConusFrame, m_LocalFrame) ;
|
|
ptR2.LocToLoc( ConusFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ptR2.z) ;
|
|
dMax = max( ptR1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= dl && ptR1.x < dL && ptR2.x >= dL) {
|
|
|
|
ptR1.LocToLoc( ConusFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR1.z, ( dDotB - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
// Fresatura
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::Milling( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
|
|
if ( m_nToolType == CylindricalMill)
|
|
|
|
return MillCyl( ptLs, ptLe, vtToolDir, m_dHeight, m_dRadius) ;
|
|
|
|
else if ( m_nToolType == BallEndMill) {
|
|
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
|
|
MillCyl( ptLs, ptLe, vtToolDir, dCylH, m_dRadius) ;
|
|
|
|
Point3d ptBs = ptLs - vtToolDir * ( m_dHeight - m_dTipHeight) ;
|
|
Point3d ptBe = ptLe - vtToolDir * ( m_dHeight - m_dTipHeight) ;
|
|
|
|
Ball( ptBs, ptBe, m_dRadius) ;
|
|
return true ;
|
|
}
|
|
|
|
else if ( m_nToolType == BullNoseMill)
|
|
// Caso al momento non gestito
|
|
return false ;
|
|
|
|
else if ( m_nToolType == ConusMill) {
|
|
|
|
double dCylH = m_dHeight - m_dTipHeight ;
|
|
|
|
MillCyl( ptLs, ptLe, vtToolDir, dCylH, m_dRadius) ;
|
|
|
|
double dMinRad = ( m_dRadius > m_dTipRadius ? m_dTipRadius : m_dRadius) ;
|
|
double dMaxRad = ( m_dRadius > m_dTipRadius ? m_dRadius : m_dTipRadius) ;
|
|
|
|
Point3d ptCs = ( m_dRadius > m_dTipRadius ? ptLs - dCylH * vtToolDir : ptLs - m_dHeight * vtToolDir) ;
|
|
Point3d ptCe = ( m_dRadius > m_dTipRadius ? ptLe - dCylH * vtToolDir : ptLe - m_dHeight * vtToolDir) ;
|
|
Vector3d vtDir = ( m_dRadius > m_dTipRadius ? vtToolDir : - vtToolDir) ;
|
|
|
|
MillConus( ptCs, ptCe, vtDir, m_dTipHeight, dMaxRad, dMinRad) ;
|
|
return true ;
|
|
}
|
|
|
|
else if ( m_nToolType == GenericTool)
|
|
// Caso al momento non gestito
|
|
return false ;
|
|
|
|
else
|
|
return false ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillingGT( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir)
|
|
{
|
|
// Posizioni iniziale e finale dell'utensile
|
|
Point3d ptI = ptLs ;
|
|
Point3d ptF = ptLe ;
|
|
|
|
// vettore movimento
|
|
Vector3d vtMove = ptLe - ptLs ;
|
|
|
|
CurveComposite ToolProfile ;
|
|
int i = m_ToolArcLineApprox.GetCurveCount() ;
|
|
|
|
// Settaggio profilo
|
|
if ( m_ToolArcLineApprox.GetCurveCount() == 0)
|
|
// Se l'utensile non è stato approssimato uso l'originale
|
|
ToolProfile.CopyFrom( & m_ToolOutline) ;
|
|
else
|
|
// altrimenti usi l'approssimazione
|
|
ToolProfile.CopyFrom( & m_ToolArcLineApprox) ;
|
|
|
|
// Dichiaro un puntatore a curva da usare nel ciclo
|
|
const ICurve* pCurve = ToolProfile.GetFirstCurve() ;
|
|
|
|
// Ciclo sulle curve
|
|
while ( pCurve != nullptr) {
|
|
|
|
double dHeight ;
|
|
|
|
int nCurveType = pCurve -> GetType() ;
|
|
|
|
// Caso di semento
|
|
if ( nCurveType == CRV_LINE) {
|
|
|
|
Point3d ptStart, ptEnd ;
|
|
|
|
pCurve -> GetStartPoint( ptStart) ;
|
|
pCurve -> GetEndPoint( ptEnd) ;
|
|
|
|
if ( abs( ptStart.y - ptEnd.y) > EPS_SMALL) {
|
|
|
|
dHeight = abs( ptStart.y - ptEnd.y) ;
|
|
|
|
// Il componente è un cilindro
|
|
if ( abs( ptStart.x - ptEnd.x) < EPS_SMALL) {
|
|
|
|
double dRadius = ptStart.x ;
|
|
|
|
MillCyl( ptI, ptF, vtToolDir, dHeight, dRadius) ;
|
|
}
|
|
// Il componente è un cono con vettore equiverso a quello dell'utensile
|
|
else if ( ptStart.x > ptEnd.x) {
|
|
|
|
double dMaxRad = ptStart.x ;
|
|
double dMinRad = ptEnd.x ;
|
|
|
|
MillConus( ptI, ptF, vtToolDir, dHeight, dMaxRad, dMinRad) ;
|
|
}
|
|
// Il componente è un cono con vettore opposto a quello dell'utensile
|
|
else if ( ptStart.x < ptEnd.x) {
|
|
|
|
double dMaxRad = ptEnd.x ;
|
|
double dMinRad = ptStart.x ;
|
|
|
|
Point3d ptIn = ptI - vtToolDir * dHeight ;
|
|
Point3d ptFn = ptIn + vtMove ;
|
|
|
|
MillConus( ptIn, ptFn, - vtToolDir, dHeight, dMaxRad, dMinRad) ;
|
|
}
|
|
}
|
|
else
|
|
dHeight = 0 ;
|
|
}
|
|
// Caso arco
|
|
else if ( nCurveType == CRV_ARC) {
|
|
|
|
// Centro e Punti iniziale e finale del cerchio
|
|
Point3d ptStart, ptEnd, ptO ;
|
|
|
|
pCurve -> GetStartPoint( ptStart) ;
|
|
pCurve -> GetEndPoint( ptEnd) ;
|
|
pCurve -> GetCenterPoint( ptO) ;
|
|
|
|
// Determino il raggio
|
|
Vector3d vtStRad = ptStart - ptO ;
|
|
Vector3d vtEnRad = ptEnd - ptO ;
|
|
|
|
double dRadius = 0.5 * ( vtStRad.LenXY() + vtEnRad.LenXY()) ;
|
|
|
|
// Determino le posizioni iniziale e finale del centrodella sfera
|
|
Point3d ptOSt = ptI - vtToolDir * ( ptStart.y - ptO.y) ;
|
|
Point3d ptOEn = ptOSt + vtMove ;
|
|
|
|
// Eseguo l'asportazione del materiale
|
|
Ball( ptOSt, ptOEn, dRadius) ;
|
|
|
|
|
|
// aggiorno l'altezza
|
|
dHeight = abs( ptStart.y - ptEnd.y) ;
|
|
}
|
|
|
|
// Determino le posizioni iniziale e finale del componente successivo
|
|
ptI = ptI - vtToolDir * dHeight ;
|
|
ptF = ptI + vtMove ;
|
|
|
|
// Aggiorno il puntatore
|
|
pCurve = ToolProfile.GetNextCurve() ;
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
// Componenti elementari degli utensili
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillCyl( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir, double dHei, double dRad)
|
|
{
|
|
double dMin, dMax ;
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BBoxComponent( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ, dRad, dRad, dHei) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
Point3d ptI ;
|
|
Point3d ptF ;
|
|
Vector3d vtV1 ;
|
|
|
|
// Studio delle simmetrie
|
|
if ( vtToolDir.z < 0) {
|
|
|
|
vtV1 = - vtToolDir ;
|
|
|
|
ptI = ( vtV1 * ( ptLe - ptLs) > 0 ? ptLs + dHei * vtV1 : ptLe + dHei * vtV1) ;
|
|
ptF = ( vtV1 * ( ptLe - ptLs) > 0 ? ptLe + dHei * vtV1 : ptLs + dHei * vtV1) ;
|
|
}
|
|
else {
|
|
|
|
vtV1 = vtToolDir ;
|
|
|
|
ptI = ( vtV1 * ( ptLe - ptLs) > 0 ? ptLs : ptLe) ;
|
|
ptF = ( vtV1 * ( ptLe - ptLs) > 0 ? ptLe : ptLs) ;
|
|
}
|
|
|
|
Point3d ptIT = ptI - vtV1 * dHei ;
|
|
Point3d ptFT = ptF - vtV1 * dHei ;
|
|
|
|
// Definizione di un sintema di riferimento nel piano
|
|
Vector3d vtU1( - vtV1.x, - vtV1.y, 0) ;
|
|
double dCos = vtV1.z ;
|
|
double dSin = vtU1.LenXY() ;
|
|
vtU1.Normalize() ; // Ocio che la sua lunghezza sia maggiore di EPS_SMALL
|
|
Vector3d vtU2 = vtU1 ; vtU2.Rotate( Z_AX, 90) ;
|
|
|
|
double dZI = ptI.z ;
|
|
double dZF = ptF.z ;
|
|
double dDeltaZ = ptIT.z - ptI.z ;
|
|
//double dDeltaFz = ptFT.z - ptF.z ;
|
|
|
|
double dL = dSin * dHei ;
|
|
|
|
Vector3d vtMove = ptF - ptI ; double dLen = vtMove.Len() ;
|
|
Vector3d vtMLong = ( vtMove * vtV1) * vtV1 ; double dLLong = vtMLong.Len() ;
|
|
Vector3d vtMOrt = vtMove - vtMLong ; double dLOrt = vtMOrt.Len() ;
|
|
|
|
double dCoef = dLOrt / dLLong ;
|
|
double dAng = atan( 1 / dCoef) ;
|
|
|
|
// vtV1, vtV2 e vtV3 definiscono gli assi dei sistemi di riferimento intrinseci
|
|
Vector3d vtV2 = vtMOrt ; vtV2.Normalize() ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
|
|
Frame3d CylFrame ; CylFrame.Set( ptI, vtV1, vtV2, vtV3) ;
|
|
Frame3d FCylFrame ; FCylFrame.Set( ptF, vtV1, vtV2, vtV3) ;
|
|
Frame3d TCylFrame ; TCylFrame.Set( ptIT, vtV1, vtV2, vtV3) ;
|
|
Frame3d FTCylFrame ; FTCylFrame.Set( ptFT, vtV1, vtV2, vtV3) ;
|
|
|
|
// Altri punti notevoli
|
|
Point3d ptIPlus = ptI + dRad * vtV3 ;
|
|
Point3d ptIMinus = ptI - dRad * vtV3 ;
|
|
Point3d ptFPlus = ptIPlus + vtMove ;
|
|
Point3d ptFMinus = ptIMinus + vtMove ;
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
Point3d ptFxy( ptF.x, ptF.y, 0) ;
|
|
|
|
// Grandezze per la definizione dei piani
|
|
Vector3d vtRI = ptI - ORIG ; double dDotI = vtV1 * vtRI ;
|
|
Vector3d vtRF = ptF - ORIG ; double dDotF = vtV1 * vtRF ;
|
|
|
|
Vector3d vtRIT = ptIT - ORIG ; double dDotIT = vtV1 * vtRIT ;
|
|
Vector3d vtRFT = ptFT - ORIG ; double dDotFT = vtV1 * vtRFT ;
|
|
|
|
Vector3d vtRIPlus = ptIPlus - ORIG ;
|
|
|
|
Vector3d vtRIMinus = ptIMinus - ORIG ;
|
|
|
|
Vector3d vtRFPlus = ptFPlus - ORIG ;
|
|
|
|
Vector3d vtW1 = vtV1 ; vtW1.Rotate( vtV3, - 180 * dAng / PIGRECO) ;
|
|
Vector3d vtW2 = vtV2 ; vtW2.Rotate( vtV3, - 180 * dAng / PIGRECO) ;
|
|
|
|
Vector3d vtRITPlus = ptIPlus - ORIG - vtV1 * dHei ;
|
|
|
|
Frame3d RotFrame ; RotFrame.Set( ptI, vtW1, vtW2, vtV3) ;
|
|
Frame3d TRotFrame ; TRotFrame.Set( ptIT, vtW1, vtW2, vtV3) ;
|
|
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ;
|
|
double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ;
|
|
|
|
Vector3d vtCI = ptC - ptIxy ;
|
|
Vector3d vtCF = ptC - ptFxy ;
|
|
Vector3d vtC = ptC - ORIG ;
|
|
|
|
double dPI1 = vtCI * vtU1 ; double dPI2 = vtCI * vtU2 ;
|
|
double dPF1 = vtCF * vtU1 ; double dPF2 = vtCF * vtU2 ;
|
|
|
|
// Forse queste parti cilindriche andrebbero fatte per intersezione se il versoreutensile è molto verticale
|
|
// Parte cilindrica I
|
|
if ( dPI1 > - dCos * sqrt( dRad * dRad - dPI2 * dPI2) &&
|
|
dPI1 < dL + dCos * sqrt( dRad * dRad - dPI2 * dPI2) &&
|
|
dPI2 * dPI2 < dRad * dRad) {
|
|
|
|
// Minimi
|
|
if ( dPI1 < dL - dCos * sqrt( dRad * dRad - dPI2 * dPI2)) {
|
|
|
|
double dZ0 = - dSin * sqrt( dRad * dRad - dPI2 * dPI2) ;
|
|
double dI10 = - dCos * sqrt( dRad * dRad - dPI2 * dPI2) ;
|
|
|
|
dMin = dZI + dZ0 + ( dPI1 - dI10) * dDeltaZ / dL ;
|
|
}
|
|
else
|
|
|
|
dMin = ( dDotIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
// Massimi
|
|
if ( dPI1 < dCos * sqrt( dRad * dRad - dPI2 * dPI2))
|
|
|
|
dMax = ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
else {
|
|
|
|
double dZ0 = dSin * sqrt( dRad * dRad - dPI2 * dPI2) ;
|
|
double dI10 = dCos * sqrt( dRad * dRad - dPI2 * dPI2) ;
|
|
|
|
dMax = dZI + dZ0 + ( dPI1 - dI10) * dDeltaZ / dL ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
// Parte cilindrica F
|
|
if ( dPF1 > - dCos * sqrt( dRad * dRad - dPF2 * dPF2) &&
|
|
dPF1 < dL + dCos * sqrt( dRad * dRad - dPF2 * dPF2) &&
|
|
dPF2 * dPF2 < dRad * dRad) {
|
|
|
|
// Minimi
|
|
if ( dPF1 < dL - dCos * sqrt( dRad * dRad - dPF2 * dPF2)) {
|
|
|
|
double dZ0 = - dSin * sqrt( dRad * dRad - dPF2 * dPF2) ;
|
|
double dI10 = - dCos * sqrt( dRad * dRad - dPF2 * dPF2) ;
|
|
|
|
dMin = dZF + dZ0 + ( dPF1 - dI10) * dDeltaZ / dL ;
|
|
}
|
|
else
|
|
|
|
dMin = ( dDotFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
// Massimi
|
|
if ( dPF1 < dCos * sqrt( dRad * dRad - dPF2 * dPF2))
|
|
|
|
dMax = ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
else {
|
|
|
|
double dZ0 = dSin * sqrt( dRad * dRad - dPF2 * dPF2) ;
|
|
double dI10 = dCos * sqrt( dRad * dRad - dPF2 * dPF2) ;
|
|
|
|
dMax = dZF + dZ0 + ( dPF1 - dI10) * dDeltaZ / dL ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
// Parallelepipedo
|
|
Point3d ptInt1 = ptC + ( ( ( vtRIPlus - vtC) * vtV3) / ( Z_AX * vtV3)) * Z_AX ;
|
|
Point3d ptInt2 = ptC + ( ( ( vtRIMinus - vtC) * vtV3) / ( Z_AX * vtV3)) * Z_AX ;
|
|
Point3d ptInt3 = ptC + ( ( ( vtRIPlus - vtC) * vtV2) / ( Z_AX * vtV2)) * Z_AX ;
|
|
Point3d ptInt4 = ptC + ( ( ( vtRIPlus - vtC) * vtW1) / ( Z_AX * vtW1)) * Z_AX ;
|
|
Point3d ptInt5 = ptC + ( ( ( vtRFPlus - vtC) * vtV2) / ( Z_AX * vtV2)) * Z_AX ;
|
|
Point3d ptInt6 = ptC + ( ( ( vtRITPlus - vtC) * vtW1) / ( Z_AX * vtW1)) * Z_AX ;
|
|
|
|
|
|
ptInt1.LocToLoc( m_LocalFrame, CylFrame) ;
|
|
ptInt2.LocToLoc( m_LocalFrame, CylFrame) ;
|
|
ptInt3.LocToLoc( m_LocalFrame, CylFrame) ;
|
|
ptInt4.LocToLoc( m_LocalFrame, RotFrame) ;
|
|
ptInt5.LocToLoc( m_LocalFrame, CylFrame) ;
|
|
ptInt6.LocToLoc( m_LocalFrame, TRotFrame) ;
|
|
|
|
bool bFlag = false ;
|
|
double dLim1, dLim2 ;
|
|
|
|
if ( ptInt1.y >= 0 && ptInt1.y <= dLOrt &&
|
|
ptInt1.x >= - dHei + ptInt1.y * ( dLLong / dLOrt) &&
|
|
ptInt1.x <= ptInt1.y * ( dLLong / dLOrt)) {
|
|
|
|
ptInt1.LocToLoc( CylFrame, m_LocalFrame) ;
|
|
|
|
|
|
dLim1 = ptInt1.z ;
|
|
bFlag = true ;
|
|
}
|
|
|
|
if ( ptInt2.y >= 0 && ptInt2.y <= dLOrt &&
|
|
ptInt2.x >= - dHei + ptInt2.y * ( dLLong / dLOrt) &&
|
|
ptInt2.x <= ptInt2.y * ( dLLong / dLOrt)) {
|
|
|
|
ptInt2.LocToLoc( CylFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt2.z ;
|
|
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt2.z ;
|
|
}
|
|
|
|
if ( ptInt3.z >= - dRad && ptInt3.z <= dRad &&
|
|
ptInt3.x >= - dHei && ptInt3.x <= 0) {
|
|
|
|
ptInt3.LocToLoc( CylFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt3.z ;
|
|
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt3.z ;
|
|
}
|
|
|
|
if ( ptInt4.z >= - dRad && ptInt4.z <= dRad &&
|
|
ptInt4.y >= 0 && ptInt4.y <= dLen) {
|
|
|
|
ptInt4.LocToLoc( RotFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt4.z ;
|
|
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt4.z ;
|
|
}
|
|
|
|
if ( ptInt5.z >= - dRad && ptInt5.z <= dRad &&
|
|
ptInt5.x >= dLLong- dHei && ptInt5.x <= dLLong) {
|
|
|
|
ptInt5.LocToLoc( CylFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt5.z ;
|
|
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt5.z ;
|
|
}
|
|
|
|
if ( ptInt6.z >= - dRad && ptInt6.z <= dRad &&
|
|
ptInt6.y >= 0 && ptInt6.y <= dLen) {
|
|
|
|
ptInt6.LocToLoc( TRotFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt6.z ;
|
|
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt6.z ;
|
|
}
|
|
|
|
|
|
if ( bFlag == true) { // Una linea non confinata se entra in un volume chiuso ci deve uscire
|
|
|
|
dMin = min( dLim1, dLim2) ;
|
|
dMax = max( dLim1, dLim2) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
// Traslazione dell'ellisse
|
|
|
|
Vector3d vtK = Z_AX ;
|
|
|
|
vtK.LocToLoc( m_LocalFrame, CylFrame) ;
|
|
ptC.LocToLoc( m_LocalFrame, CylFrame) ;
|
|
|
|
std::vector <double> vdCoef(3);
|
|
std::vector <double> vdRoots;
|
|
|
|
vdCoef[0] = dCoef * dCoef * ptC.x * ptC.x + ptC.y * ptC.y + ptC.z * ptC.z - 2 * dCoef * ptC.x * ptC.y - dRad * dRad ;
|
|
vdCoef[1] = 2 * ( dCoef * dCoef * vtK.x * ptC.x + vtK.y * ptC.y + vtK.z * ptC.z - dCoef * ( vtK.x * ptC.y + vtK.y * ptC.x)) ;
|
|
vdCoef[2] = dCoef * dCoef * vtK.x * vtK.x + vtK.y * vtK.y + vtK.z * vtK.z - 2 * dCoef * vtK.x * vtK.y ;
|
|
|
|
|
|
int nRoot = PolynomialRoots( 2, vdCoef, vdRoots) ;
|
|
|
|
if ( nRoot == 0 || nRoot == 1) {
|
|
|
|
Point3d ptPi ; ptPi.x = dX ; ptPi.y = dY ; ptPi.z = ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
Point3d ptPf ; ptPf.x = dX ; ptPf.y = dY ; ptPf.z = ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
ptPi.LocToLoc( m_LocalFrame, CylFrame) ;
|
|
ptPf.LocToLoc( m_LocalFrame, FCylFrame) ;
|
|
|
|
if ( ptPi.y * ptPi.y + ptPi.z * ptPi.z < dRad * dRad &&
|
|
ptPf.y * ptPf.y + ptPf.z * ptPf.z < dRad * dRad) {
|
|
|
|
ptPi.LocToLoc( CylFrame, m_LocalFrame) ;
|
|
ptPf.LocToLoc( FCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptPi.z, ptPf.z) ;
|
|
dMax = max( ptPi.z, ptPf.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( nRoot == 2) {
|
|
|
|
Point3d ptInter1 = ptC + vdRoots[0] * vtK ;
|
|
Point3d ptInter2 = ptC + vdRoots[1] * vtK ;
|
|
|
|
|
|
if ( ptInter1.x > ptInter2.x) {
|
|
|
|
Point3d ptTemp = ptInter1 ;
|
|
ptInter1 = ptInter2 ;
|
|
ptInter2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptInter1.x > 0 && ptInter1.x < dLLong &&
|
|
ptInter2.x > dLLong) {
|
|
|
|
ptInter1.LocToLoc( CylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter1.z) ;
|
|
dMax = max( ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptInter1.x > 0 && ptInter2.x < dLLong) {
|
|
|
|
ptInter1.LocToLoc( CylFrame, m_LocalFrame) ;
|
|
ptInter2.LocToLoc( CylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptInter1.z, ptInter2.z) ;
|
|
dMax = max( ptInter1.z, ptInter2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptInter1.x < 0 && ptInter2.x > dLLong) {
|
|
|
|
dMin = min( ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptInter1.x < 0 && ptInter2.x > 0 && ptInter2.x < dLLong) {
|
|
|
|
ptInter2.LocToLoc( CylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter2.z) ;
|
|
dMax = max( ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
ptC.LocToLoc( CylFrame, TCylFrame) ;
|
|
vtK.LocToLoc( CylFrame, TCylFrame) ;
|
|
|
|
std::vector <double> vdTCoef(3);
|
|
std::vector <double> vdTRoots;
|
|
|
|
vdTCoef[0] = dCoef * dCoef * ptC.x * ptC.x + ptC.y * ptC.y + ptC.z * ptC.z - 2 * dCoef * ptC.x * ptC.y - dRad * dRad ;
|
|
vdTCoef[1] = 2 * ( dCoef * dCoef * vtK.x * ptC.x + vtK.y * ptC.y + vtK.z * ptC.z - dCoef * ( vtK.x * ptC.y + vtK.y * ptC.x)) ;
|
|
vdTCoef[2] = dCoef * dCoef * vtK.x * vtK.x + vtK.y * vtK.y + vtK.z * vtK.z - 2 * dCoef * vtK.x * vtK.y ;
|
|
|
|
int nTRoot = PolynomialRoots( 2, vdTCoef, vdTRoots) ;
|
|
|
|
if ( nTRoot == 0 || nTRoot == 1) {
|
|
|
|
Point3d ptPi ; ptPi.x = dX ; ptPi.y = dY ; ptPi.z = ( dDotIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
Point3d ptPf ; ptPf.x = dX ; ptPf.y = dY ; ptPf.z = ( dDotFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
ptPi.LocToLoc( m_LocalFrame, TCylFrame) ;
|
|
ptPf.LocToLoc( m_LocalFrame, FTCylFrame) ;
|
|
|
|
if ( ptPi.y * ptPi.y + ptPi.z * ptPi.z < dRad * dRad &&
|
|
ptPf.y * ptPf.y + ptPf.z * ptPf.z < dRad * dRad) {
|
|
|
|
ptPi.LocToLoc( TCylFrame, m_LocalFrame) ;
|
|
ptPf.LocToLoc( FTCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptPi.z, ptPf.z) ;
|
|
dMax = max( ptPi.z, ptPf.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
}
|
|
else if ( nTRoot == 2) {
|
|
|
|
Point3d ptTInter1 = ptC + vdTRoots[0] * vtK ;
|
|
Point3d ptTInter2 = ptC + vdTRoots[1] * vtK ;
|
|
|
|
if ( ptTInter1.x > ptTInter2.x) {
|
|
|
|
Point3d ptTemp = ptTInter1 ;
|
|
ptTInter1 = ptTInter2 ;
|
|
ptTInter2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptTInter1.x > 0 && ptTInter1.x < dLLong &&
|
|
ptTInter2.x > dLLong) {
|
|
|
|
ptTInter1.LocToLoc( TCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptTInter1.z) ;
|
|
dMax = max( ( dDotFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptTInter1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptTInter1.x > 0 && ptTInter2.x < dLLong) {
|
|
|
|
ptTInter1.LocToLoc( TCylFrame, m_LocalFrame) ;
|
|
ptTInter2.LocToLoc( TCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptTInter1.z, ptTInter2.z) ;
|
|
dMax = max( ptTInter1.z, ptTInter2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptTInter1.x < 0 && ptTInter2.x > dLLong) {
|
|
|
|
dMin = min( ( dDotFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptTInter1.x < 0 && ptTInter2.x > 0 && ptTInter2.x < dLLong) {
|
|
|
|
ptTInter2.LocToLoc( TCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptTInter2.z) ;
|
|
dMax = max( ( dDotIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptTInter2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillCyl2( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir, double dHei, double dRad)
|
|
{
|
|
double dMin, dMax ;
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BBoxComponent( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ, dRad, dRad, dHei) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
// Punti notevoli
|
|
Point3d ptI = ptLs ;
|
|
Point3d ptF = ptLe ;
|
|
|
|
Point3d ptIT = ptI - vtToolDir * dHei ;
|
|
Point3d ptFT = ptF - vtToolDir * dHei ;
|
|
|
|
// Vettori notevoli
|
|
Vector3d vtMove = ptF - ptI ; double dLen = vtMove.Len() ;
|
|
Vector3d vtLong = ( vtMove * vtToolDir) * vtToolDir ; double dLong = vtLong.Len() ;
|
|
Vector3d vtOrt = vtMove - vtLong ; double dOrt = vtOrt.Len() ;
|
|
|
|
double dCoef = dOrt / dLong ;
|
|
double dAng = atan( 1 / dCoef) ;
|
|
|
|
Vector3d vtV1 = vtToolDir ;
|
|
Vector3d vtV2 = vtOrt ; vtV2.Normalize() ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
|
|
// Definizione dei sistemi di riferimento
|
|
Frame3d ICylFrame ; ICylFrame.Set( ptI, vtV1, vtV2, vtV3) ;
|
|
Frame3d FCylFrame ; FCylFrame.Set( ptF, vtV1, vtV2, vtV3) ;
|
|
Frame3d ITCylFrame ; ITCylFrame.Set( ptIT, vtV1, vtV2, vtV3) ;
|
|
Frame3d FTCylFrame ; FTCylFrame.Set( ptFT, vtV1, vtV2, vtV3) ;
|
|
|
|
Vector3d vtW1 = vtV1 ; vtW1.Rotate( vtV3, - 180 * dAng / PIGRECO) ;
|
|
Vector3d vtW2 = vtV2 ; vtW2.Rotate( vtV3, - 180 * dAng / PIGRECO) ;
|
|
Vector3d vtW3 = vtV3 ;
|
|
|
|
Frame3d RotFrame ; RotFrame.Set( ptI, vtW1, vtW2, vtW3) ;
|
|
Frame3d TRotFrame ; TRotFrame.Set( ptIT, vtW1, vtW2, vtW3) ;
|
|
|
|
// Altri vettori notevoi
|
|
Vector3d vtKC = Z_AX ; vtKC.LocToLoc( m_LocalFrame, ICylFrame) ;
|
|
|
|
Vector3d vtRI = ptI - ORIG ; double dDotI = vtRI * vtV1 ;
|
|
Vector3d vtRF = ptF - ORIG ; double dDotF = vtRF * vtV1 ;
|
|
Vector3d vtRIT = ptIT - ORIG ; double dDotIT = vtRIT * vtV1 ;
|
|
Vector3d vtRFT = ptFT - ORIG ; double dDotFT = vtRFT * vtV1 ;
|
|
|
|
Vector3d vtRIPlus = vtRI + dRad * vtW3 ;
|
|
Vector3d vtRIMinus = vtRI - dRad * vtW3 ;
|
|
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ORIG ;
|
|
|
|
// Cilindro iniziale
|
|
ptC.LocToLoc( m_LocalFrame, ICylFrame) ;
|
|
|
|
std::vector <double> vdICylCoef(3);
|
|
std::vector <double> vdICylRoots;
|
|
|
|
vdICylCoef[0] = ptC.y * ptC.y + ptC.z * ptC.z - dRad * dRad ;
|
|
vdICylCoef[1] = 2 * ( ptC.y * vtKC.y + ptC.z * vtKC.z) ;
|
|
vdICylCoef[2] = vtKC.y * vtKC.y + vtKC.z * vtKC.z ;
|
|
|
|
|
|
int nICylRoot = PolynomialRoots( 2, vdICylCoef, vdICylRoots) ;
|
|
|
|
if ( nICylRoot == 0 || nICylRoot == 1) {
|
|
|
|
Point3d ptPb ; ptPb.x = dX ; ptPb.y = dY ; ptPb.z = ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
Point3d ptPt ; ptPt.x = dX ; ptPt.y = dY ; ptPt.z = ( dDotIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
ptPb.LocToLoc( m_LocalFrame, ICylFrame) ;
|
|
ptPt.LocToLoc( m_LocalFrame, ITCylFrame) ;
|
|
|
|
if ( ptPb.y * ptPb.y + ptPb.z * ptPb.z < dRad * dRad &&
|
|
ptPt.y * ptPt.y + ptPt.z * ptPt.z < dRad * dRad) {
|
|
|
|
ptPb.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
ptPt.LocToLoc( ITCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptPb.z, ptPt.z) ;
|
|
dMax = max( ptPb.z, ptPt.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( nICylRoot == 2) {
|
|
|
|
Point3d ptR1 = ptC + vdICylRoots[0] * vtKC ;
|
|
Point3d ptR2 = ptC + vdICylRoots[1] * vtKC ;
|
|
|
|
if ( ptR1.x > ptR2.x) {
|
|
|
|
Point3d ptTemp = ptR1 ;
|
|
ptR1 = ptR2 ;
|
|
ptR2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptR1.x < - dHei && ptR2.x >= - dHei &&
|
|
ptR2.x < 0) {
|
|
|
|
ptR2.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotIT - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR2.z) ;
|
|
dMax = max( ( dDotIT - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x < - dHei && ptR2.x >= 0) {
|
|
|
|
dMin = min( ( dDotI - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ( dDotIT - dX * vtV1.x - dY * vtV1.y) / vtV1.z) ;
|
|
dMax = max( ( dDotI - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ( dDotIT - dX * vtV1.x - dY * vtV1.y) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= - dHei && ptR1.x < 0 && ptR2.x < 0) {
|
|
|
|
ptR1.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
ptR2.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ptR2.z) ;
|
|
dMax = max( ptR1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= - dHei && ptR1.x < 0 && ptR2.x >= 0) {
|
|
|
|
ptR1.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotI - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR1.z) ;
|
|
dMax = max( ( dDotI - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
// Cilindro finale
|
|
ptC.LocToLoc( ICylFrame, FCylFrame) ;
|
|
|
|
std::vector <double> vdFCylCoef(3);
|
|
std::vector <double> vdFCylRoots;
|
|
|
|
vdFCylCoef[0] = ptC.y * ptC.y + ptC.z * ptC.z - dRad * dRad ;
|
|
vdFCylCoef[1] = 2 * ( ptC.y * vtKC.y + ptC.z * vtKC.z) ;
|
|
vdFCylCoef[2] = vtKC.y * vtKC.y + vtKC.z * vtKC.z ;
|
|
|
|
|
|
int nFCylRoot = PolynomialRoots( 2, vdFCylCoef, vdFCylRoots) ;
|
|
|
|
if ( nFCylRoot == 0 || nFCylRoot == 1) {
|
|
|
|
Point3d ptPb ; ptPb.x = dX ; ptPb.y = dY ; ptPb.z = ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
Point3d ptPt ; ptPt.x = dX ; ptPt.y = dY ; ptPt.z = ( dDotFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
ptPb.LocToLoc( m_LocalFrame, FCylFrame) ;
|
|
ptPt.LocToLoc( m_LocalFrame, FTCylFrame) ;
|
|
|
|
if ( ptPb.y * ptPb.y + ptPb.z * ptPb.z < dRad * dRad &&
|
|
ptPt.y * ptPt.y + ptPt.z * ptPt.z < dRad * dRad) {
|
|
|
|
ptPb.LocToLoc( FCylFrame, m_LocalFrame) ;
|
|
ptPt.LocToLoc( FTCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptPb.z, ptPt.z) ;
|
|
dMax = max( ptPb.z, ptPt.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( nFCylRoot == 2) {
|
|
|
|
Point3d ptR1 = ptC + vdFCylRoots[0] * vtKC ;
|
|
Point3d ptR2 = ptC + vdFCylRoots[1] * vtKC ;
|
|
|
|
if ( ptR1.x > ptR2.x) {
|
|
|
|
Point3d ptTemp = ptR1 ;
|
|
ptR1 = ptR2 ;
|
|
ptR2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptR1.x < - dHei && ptR2.x >= - dHei &&
|
|
ptR2.x < 0) {
|
|
|
|
ptR2.LocToLoc( FCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotFT - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR2.z) ;
|
|
dMax = max( ( dDotFT - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x < - dHei && ptR2.x >= 0) {
|
|
|
|
dMin = min( ( dDotF - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ( dDotFT - dX * vtV1.x - dY * vtV1.y) / vtV1.z) ;
|
|
dMax = max( ( dDotF - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ( dDotFT - dX * vtV1.x - dY * vtV1.y) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= - dHei && ptR1.x < 0 && ptR2.x < 0) {
|
|
|
|
ptR1.LocToLoc( FCylFrame, m_LocalFrame) ;
|
|
ptR2.LocToLoc( FCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ptR2.z) ;
|
|
dMax = max( ptR1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= - dHei && ptR1.x < 0 && ptR2.x >= 0) {
|
|
|
|
ptR1.LocToLoc( FCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotF - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR1.z) ;
|
|
dMax = max( ( dDotF - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
// Traslazione ellisse fondo
|
|
|
|
ptC.LocToLoc( FCylFrame, ICylFrame) ;
|
|
|
|
std::vector <double> vdEllipseCoef(3);
|
|
std::vector <double> vdEllipseRoots;
|
|
|
|
vdEllipseCoef[0] = dCoef * dCoef * ptC.x * ptC.x + ptC.y * ptC.y + ptC.z * ptC.z - 2 * dCoef * ptC.x * ptC.y - dRad * dRad ;
|
|
vdEllipseCoef[1] = 2 * ( dCoef * dCoef * vtKC.x * ptC.x + vtKC.y * ptC.y + vtKC.z * ptC.z - dCoef * ( vtKC.x * ptC.y + vtKC.y * ptC.x)) ;
|
|
vdEllipseCoef[2] = dCoef * dCoef * vtKC.x * vtKC.x + vtKC.y * vtKC.y + vtKC.z * vtKC.z - 2 * dCoef * vtKC.x * vtKC.y ;
|
|
|
|
|
|
int nEllipseRoot = PolynomialRoots( 2, vdEllipseCoef, vdEllipseRoots) ;
|
|
|
|
if ( nEllipseRoot == 0 || nEllipseRoot == 1) {
|
|
|
|
Point3d ptPi ; ptPi.x = dX ; ptPi.y = dY ; ptPi.z = ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
Point3d ptPf ; ptPf.x = dX ; ptPf.y = dY ; ptPf.z = ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
ptPi.LocToLoc( m_LocalFrame, ICylFrame) ;
|
|
ptPf.LocToLoc( m_LocalFrame, FCylFrame) ;
|
|
|
|
if ( ptPi.y * ptPi.y + ptPi.z * ptPi.z < dRad * dRad &&
|
|
ptPf.y * ptPf.y + ptPf.z * ptPf.z < dRad * dRad) {
|
|
|
|
ptPi.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
ptPf.LocToLoc( FCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptPi.z, ptPf.z) ;
|
|
dMax = max( ptPi.z, ptPf.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( nEllipseRoot == 2) {
|
|
|
|
Point3d ptR1 = ptC + vdEllipseRoots[0] * vtKC ;
|
|
Point3d ptR2 = ptC + vdEllipseRoots[1] * vtKC ;
|
|
|
|
if ( ptR1.x > ptR2.x) {
|
|
|
|
Point3d ptTemp = ptR1 ;
|
|
ptR1 = ptR2 ;
|
|
ptR2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptR1.x < 0 && ptR2.x >= 0 &&
|
|
ptR2.x < dLong) {
|
|
|
|
ptR2.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotI - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR2.z) ;
|
|
dMax = max( ( dDotI - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x < 0 && ptR2.x >= dLong) {
|
|
|
|
dMin = min( ( dDotI - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ( dDotF - dX * vtV1.x - dY * vtV1.y) / vtV1.z) ;
|
|
dMax = max( ( dDotI - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ( dDotF - dX * vtV1.x - dY * vtV1.y) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= 0 && ptR1.x < dLong && ptR2.x < dLong) {
|
|
|
|
ptR1.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
ptR2.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ptR2.z) ;
|
|
dMax = max( ptR1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= 0 && ptR1.x < dLong && ptR2.x >= dLong) {
|
|
|
|
ptR1.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotF - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR1.z) ;
|
|
dMax = max( ( dDotF - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
// Traslazione ellisse punta
|
|
|
|
ptC.LocToLoc( ICylFrame, ITCylFrame) ;
|
|
|
|
std::vector <double> vdTEllipseCoef(3);
|
|
std::vector <double> vdTEllipseRoots;
|
|
|
|
vdTEllipseCoef[0] = dCoef * dCoef * ptC.x * ptC.x + ptC.y * ptC.y + ptC.z * ptC.z - 2 * dCoef * ptC.x * ptC.y - dRad * dRad ;
|
|
vdTEllipseCoef[1] = 2 * ( dCoef * dCoef * vtKC.x * ptC.x + vtKC.y * ptC.y + vtKC.z * ptC.z - dCoef * ( vtKC.x * ptC.y + vtKC.y * ptC.x)) ;
|
|
vdTEllipseCoef[2] = dCoef * dCoef * vtKC.x * vtKC.x + vtKC.y * vtKC.y + vtKC.z * vtKC.z - 2 * dCoef * vtKC.x * vtKC.y ;
|
|
|
|
|
|
int nTEllipseRoot = PolynomialRoots( 2, vdTEllipseCoef, vdTEllipseRoots) ;
|
|
|
|
if ( nTEllipseRoot == 0 || nTEllipseRoot == 1) {
|
|
|
|
Point3d ptPi ; ptPi.x = dX ; ptPi.y = dY ; ptPi.z = ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
Point3d ptPf ; ptPf.x = dX ; ptPf.y = dY ; ptPf.z = ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
ptPi.LocToLoc( m_LocalFrame, ITCylFrame) ;
|
|
ptPf.LocToLoc( m_LocalFrame, FTCylFrame) ;
|
|
|
|
if ( ptPi.y * ptPi.y + ptPi.z * ptPi.z < dRad * dRad &&
|
|
ptPf.y * ptPf.y + ptPf.z * ptPf.z < dRad * dRad) {
|
|
|
|
ptPi.LocToLoc( ITCylFrame, m_LocalFrame) ;
|
|
ptPf.LocToLoc( FTCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptPi.z, ptPf.z) ;
|
|
dMax = max( ptPi.z, ptPf.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( nTEllipseRoot == 2) {
|
|
|
|
Point3d ptR1 = ptC + vdTEllipseRoots[0] * vtKC ;
|
|
Point3d ptR2 = ptC + vdTEllipseRoots[1] * vtKC ;
|
|
|
|
if ( ptR1.x > ptR2.x) {
|
|
|
|
Point3d ptTemp = ptR1 ;
|
|
ptR1 = ptR2 ;
|
|
ptR2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptR1.x < 0 && ptR2.x >= 0 &&
|
|
ptR2.x < dLong) {
|
|
|
|
ptR2.LocToLoc( ITCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotIT - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR2.z) ;
|
|
dMax = max( ( dDotIT - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x < 0 && ptR2.x >= dLong) {
|
|
|
|
dMin = min( ( dDotIT - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ( dDotFT - dX * vtV1.x - dY * vtV1.y) / vtV1.z) ;
|
|
dMax = max( ( dDotIT - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ( dDotFT - dX * vtV1.x - dY * vtV1.y) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= 0 && ptR1.x < dLong && ptR2.x < dLong) {
|
|
|
|
ptR1.LocToLoc( ITCylFrame, m_LocalFrame) ;
|
|
ptR2.LocToLoc( ITCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ptR2.z) ;
|
|
dMax = max( ptR1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= 0 && ptR1.x < dLong && ptR2.x >= dLong) {
|
|
|
|
ptR1.LocToLoc( ITCylFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotFT - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR1.z) ;
|
|
dMax = max( ( dDotFT - dX * vtV1.x - dY * vtV1.y) / vtV1.z, ptR1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
|
|
// Parallelepipedo
|
|
Point3d ptInt1 = ptC + ( ( ( vtRI - vtC) * vtV2) / ( Z_AX * vtV2)) * Z_AX ;
|
|
Point3d ptInt2 = ptC + ( ( ( vtRI - vtC) * vtW1) / ( Z_AX * vtW1)) * Z_AX ;
|
|
Point3d ptInt3 = ptC + ( ( ( vtRF - vtC) * vtV2) / ( Z_AX * vtV2)) * Z_AX ;
|
|
Point3d ptInt4 = ptC + ( ( ( vtRIT - vtC) * vtW1) / ( Z_AX * vtW1)) * Z_AX ;
|
|
Point3d ptInt5 = ptC + ( ( ( vtRIPlus - vtC) * vtV3) / ( Z_AX * vtV3)) * Z_AX ;
|
|
Point3d ptInt6 = ptC + ( ( ( vtRIMinus - vtC) * vtV3) / ( Z_AX * vtV3)) * Z_AX ;
|
|
|
|
|
|
ptInt1.LocToLoc( m_LocalFrame, ICylFrame) ;
|
|
ptInt2.LocToLoc( m_LocalFrame, RotFrame) ;
|
|
ptInt3.LocToLoc( m_LocalFrame, FCylFrame) ;
|
|
ptInt4.LocToLoc( m_LocalFrame, TRotFrame) ;
|
|
ptInt5.LocToLoc( m_LocalFrame, ICylFrame) ;
|
|
ptInt6.LocToLoc( m_LocalFrame, ICylFrame) ;
|
|
|
|
bool bFlag = false ;
|
|
double dLim1, dLim2 ;
|
|
|
|
|
|
if ( ptInt1.x >= - dHei && ptInt1.x <= 0 &&
|
|
ptInt1.z >= - dRad && ptInt1.z <= dRad) {
|
|
|
|
ptInt1.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
|
|
dLim1 = ptInt1.z ;
|
|
bFlag = true ;
|
|
}
|
|
|
|
if ( ptInt2.y >= 0 && ptInt2.y <= dLen &&
|
|
ptInt2.z >= - dRad && ptInt2.z <= dRad) {
|
|
|
|
ptInt2.LocToLoc( RotFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt2.z ;
|
|
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt2.z ;
|
|
}
|
|
|
|
if ( ptInt3.z >= - dRad && ptInt3.z <= dRad &&
|
|
ptInt3.x >= - dHei && ptInt3.x <= 0) {
|
|
|
|
ptInt3.LocToLoc( FCylFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt3.z ;
|
|
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt3.z ;
|
|
}
|
|
|
|
if ( ptInt4.z >= - dRad && ptInt4.z <= dRad &&
|
|
ptInt4.y >= 0 && ptInt4.y <= dLen) {
|
|
|
|
ptInt4.LocToLoc( TRotFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt4.z ;
|
|
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt4.z ;
|
|
}
|
|
|
|
if ( ptInt5.y >= 0 && ptInt5.y <= dOrt &&
|
|
ptInt5.x >= - dHei + dCoef * ptInt5.y &&
|
|
ptInt5.x <= dCoef * ptInt5.y) {
|
|
|
|
ptInt5.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt5.z ;
|
|
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt5.z ;
|
|
}
|
|
|
|
if ( ptInt6.y >= 0 && ptInt6.y <= dOrt &&
|
|
ptInt6.x >= - dHei + dCoef * ptInt6.y &&
|
|
ptInt6.x <= dCoef * ptInt6.y) {
|
|
|
|
ptInt6.LocToLoc( ICylFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt6.z ;
|
|
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt6.z ;
|
|
}
|
|
|
|
|
|
if ( bFlag == true) { // Una linea non confinata se entra in un volume chiuso ci deve uscire
|
|
|
|
dMin = min( dLim1, dLim2) ;
|
|
dMax = max( dLim1, dLim2) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillBall( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir, double dRad)
|
|
{
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
bool bControl = BBoxComponent( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ, dRad, 0, 0) ;
|
|
if ( ! bControl)
|
|
return true ;
|
|
|
|
Point3d ptI = ( ptLs.z < ptLe.z ? ptLs : ptLe) ;
|
|
Point3d ptF = ( ptLs.z < ptLe.z ? ptLe : ptLs) ;
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
|
|
Vector3d vtMove = ptF - ptI ;
|
|
|
|
Vector3d vtV1( vtMove.x, vtMove.y, 0) ; vtV1.Normalize() ;
|
|
Vector3d vtV2( vtV1) ; vtV2.Rotate( Z_AX, 90) ;
|
|
|
|
double dZI = ptI.z ;
|
|
double dDeltaZ = ptF.z - ptI.z ;
|
|
double dPLen = vtMove.LenXY() ;
|
|
|
|
double dSin = dPLen / vtMove.Len() ;
|
|
double dCos = dDeltaZ / vtMove.Len() ;
|
|
|
|
double dMin, dMax ;
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dP1 = vtC * vtV1 ; double dP2 = vtC * vtV2 ;
|
|
|
|
if ( ( dP1 >= 0 && dP1 <= dPLen && abs( dP2) < dRad) ||
|
|
( dP1 * dP1 + dP2 * dP2 < dRad * dRad) ||
|
|
( ( dP1 - dPLen) * ( dP1 - dPLen) + dP2 * dP2 < dRad * dRad)) {
|
|
|
|
// Massimi
|
|
if ( dP1 < - dRad * dCos * sqrt( 1 - ( dP2 * dP2) / ( dRad * dRad))) {
|
|
|
|
double dH = sqrt( dRad * dRad - dP1 * dP1 - dP2 * dP2) ;
|
|
dMax = dZI + dH ;
|
|
}
|
|
else if ( dP1 < dPLen - dRad * dCos * sqrt( 1 - ( dP2 * dP2) / ( dRad * dRad))) {
|
|
|
|
double dP0 = - dRad * dCos * sqrt( 1 - ( dP2 * dP2) / ( dRad * dRad)) ;
|
|
double dZ0 = dRad * dSin * sqrt( 1 - ( dP2 * dP2) / ( dRad * dRad)) ;
|
|
|
|
dMax = dZI + dZ0 + ( dP1 - dP0) * dDeltaZ / dPLen ;
|
|
}
|
|
else {
|
|
|
|
double dH = sqrt( dRad * dRad - ( dP1 - dPLen) * ( dP1 - dPLen) - dP2 * dP2) ;
|
|
dMax = dZI + dDeltaZ + dH ;
|
|
}
|
|
|
|
// Minimi
|
|
if ( dP1 < dRad * dCos * sqrt( 1 - ( dP2 * dP2) / ( dRad * dRad))) {
|
|
|
|
double dH = sqrt( dRad * dRad - dP1 * dP1 - dP2 * dP2) ;
|
|
dMin = dZI - dH ;
|
|
}
|
|
else if ( dP1 < dPLen + dRad * dCos * sqrt( 1 - ( dP2 * dP2) / ( dRad * dRad))) {
|
|
|
|
double dP0 = dRad * dCos * sqrt( 1 - ( dP2 * dP2) / ( dRad * dRad)) ;
|
|
double dZ0 = - dRad * dSin * sqrt( 1 - ( dP2 * dP2) / ( dRad * dRad)) ;
|
|
|
|
dMin = dZI + dZ0 + ( dP1 - dP0) * dDeltaZ / dPLen ;
|
|
}
|
|
else {
|
|
|
|
double dH = sqrt( dRad * dRad - ( dP1 - dPLen) * ( dP1 - dPLen) - dP2 * dP2) ;
|
|
dMin = dZI + dDeltaZ - dH ;
|
|
}
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillConus( const Point3d& ptLs, const Point3d& ptLe, const Vector3d& vtToolDir,
|
|
double dHei, double dMaxRad, double dMinRad)
|
|
{
|
|
double dMin, dMax ;
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BBoxComponent( ptLs, ptLe, vtToolDir, vtToolDir, nStartI, nStartJ, nEndI, nEndJ, dMaxRad, dMinRad, dHei) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
double dDeltaR = dMaxRad - dMinRad ;
|
|
|
|
Point3d ptI = ( vtToolDir * ( ptLe - ptLs) > 0 ? ptLs : ptLe) ;
|
|
Point3d ptF = ( vtToolDir * ( ptLe - ptLs) > 0 ? ptLe : ptLs) ;
|
|
|
|
Point3d ptIT = ptI - vtToolDir * dHei ;
|
|
Point3d ptFT = ptF - vtToolDir * dHei ;
|
|
|
|
double dL = ( ( dMaxRad * dHei) / ( dDeltaR)) ;
|
|
double dl = dL - dHei ;
|
|
|
|
Point3d ptIV = ptI - vtToolDir * dL ;
|
|
Point3d ptFV = ptF - vtToolDir * dL ;
|
|
|
|
Vector3d vtMove = ptF - ptI ; double dLen = vtMove.Len() ;
|
|
|
|
Vector3d vtMLong = ( vtMove * vtToolDir) * vtToolDir ; double dLLong = vtMLong.Len() ;
|
|
Vector3d vtMOrt = vtMove - vtMLong ; double dLOrt = vtMOrt.Len() ;
|
|
|
|
Vector3d vtV1 = vtToolDir ;
|
|
Vector3d vtV2 = vtMOrt ; vtV2.Normalize() ;
|
|
Vector3d vtV3 = vtV1 ^ vtV2 ;
|
|
|
|
// Apertura del cono e parametri per determinare i piani
|
|
double dTan = dDeltaR / dHei ;
|
|
double dRatio = dLLong / dLOrt ;
|
|
|
|
double dCos = dTan * dRatio ;
|
|
double dSin = ( 1 - dCos * dCos > 0 ? sqrt( 1 - dCos * dCos) : 0) ;
|
|
|
|
double dDen = sqrt( 1 + dTan * dTan) ;
|
|
double dCoef = dLOrt / dLLong ; // Per traslazione ellissi
|
|
|
|
if ( dRatio > 1 / dTan)
|
|
|
|
return MillConusAux( ptI, ptF, vtV1, vtV2, vtV3, nStartI, nStartJ, nEndI, nEndJ, dHei, dMaxRad, dMinRad, dCoef) ;
|
|
|
|
|
|
// Versori normali e prodotti scalari per per determinare i piani
|
|
// Piani laterali:
|
|
Vector3d vtNs = - ( dTan / dDen) * vtV1 + ( dCos / dDen) * vtV2 + ( dSin / dDen) * vtV3 ;
|
|
Vector3d vtNd = - ( dTan / dDen) * vtV1 + ( dCos / dDen) * vtV2 - ( dSin / dDen) * vtV3 ;
|
|
Vector3d vtRIV = ptIV - ORIG ;
|
|
// double dDots = vtRIV * vtNs ; // forse qui è meglio due punti ptIT + vtV3 * dMinRad e ptIT - vtV3 * dMinRad
|
|
// double dDotd = vtRIV * vtNd ;
|
|
|
|
Vector3d vtS1 = vtNs ;
|
|
Vector3d vtS2 = vtMove ; vtS2.Normalize() ; // double dDotTestqs = vtS1 * vtS2 ;
|
|
Vector3d vtS3 = vtS1 ^ vtS2 ;
|
|
|
|
Vector3d vtD1 = vtNd ;
|
|
Vector3d vtD2 = vtS2 ; // double dDotTestD = vtD1 * vtD2 ;
|
|
Vector3d vtD3 = vtD1 ^ vtD2 ;
|
|
|
|
Point3d ptS = ptI + vtV2 * ( dMaxRad * dCos) + vtV3 * ( dMaxRad * dSin) ;
|
|
Point3d ptD = ptI + vtV2 * ( dMaxRad * dCos) - vtV3 * ( dMaxRad * dSin) ;
|
|
Point3d ptST = ptIT + vtV2 * ( dMinRad * dCos) + vtV3 * ( dMinRad * dSin) ;
|
|
|
|
Vector3d vtLen = ptST - ptS ;
|
|
|
|
double dPLong = abs( vtLen * vtS3) ;
|
|
double dPOrt = abs( vtLen * vtS2) ;
|
|
// Vector3d vtLTr = vtLen - dPLong * vtS3 ;
|
|
|
|
// double dPOrt = vtLTr.Len() ;
|
|
|
|
// Piani di fondo e punta:
|
|
Vector3d vtU1 = - dLOrt * vtV1 + dLLong * vtV2 ; vtU1.Normalize() ;
|
|
Vector3d vtU2 = vtMove ; vtU2.Normalize() ; // double dDotTest = vtU1 * vtU2 ;
|
|
Vector3d vtU3 = vtU1 ^ vtU2 ;
|
|
|
|
Point3d ptU = ptI + vtV2 * ( dMaxRad * dCos) ;
|
|
Point3d ptTU = ptIT + vtV2 * ( dMinRad * dCos) ;
|
|
|
|
Vector3d vtRU = ptU - ORIG ; // double dDotB = vtRU * vtU1 ;
|
|
Vector3d vtRUT = ptTU - ORIG ; // double dDotT = vtRUT * vtU1 ;
|
|
|
|
// Piani finale e iniziale:
|
|
Vector3d vtVAux = ptTU - ptU ;
|
|
|
|
double dAuxOrt = vtVAux * vtV2 ;
|
|
double dAuxLong = vtVAux * vtV1 ; // Tenere in considerazione per tronchi con dimensioni tali da poter approssimare tori
|
|
|
|
Vector3d vtW1 = - dAuxOrt * vtV1 + dAuxLong * vtV2 ; double dLAux1 = vtW1.Len() ; vtW1.Normalize() ;
|
|
Vector3d vtW2 = vtVAux ; double dLAux2 = vtW2.Len() ; vtW2.Normalize() ; // double dDottest = vtW1 * vtW2 ;
|
|
Vector3d vtW3 = vtW1 ^ vtW2 ;
|
|
|
|
double dPr2 = vtLen * vtW2 ; double prova1 = vtLen * vtW3 ; double prova2 = dSin * dDeltaR ;
|
|
|
|
Point3d ptFU = ptU + vtMove ;
|
|
|
|
Vector3d vtRFU = ptFU - ORIG ; // double dDotPF = vtRFU * vtW1 ;
|
|
|
|
// Piani cono:
|
|
Vector3d vtRCI = ptI - ORIG ; double dDotCI = vtRCI * vtV1 ;
|
|
Vector3d vtRCIT = ptIT - ORIG ; double dDotCIT = vtRCIT * vtV1 ;
|
|
|
|
Vector3d vtRCF = ptF - ORIG ; double dDotCF = vtRCF * vtV1 ;
|
|
Vector3d vtRCFT = ptFT - ORIG ; double dDotCFT = vtRCFT * vtV1 ;
|
|
|
|
|
|
// Sistemi di riferimento
|
|
Frame3d IConeFrame ; IConeFrame.Set( ptIV, vtV1, vtV2, vtV3) ;
|
|
Frame3d FConeFrame ; FConeFrame.Set( ptFV, vtV1, vtV2, vtV3) ;
|
|
Frame3d PlSFrame ; PlSFrame.Set( ptS, vtS1, vtS2, vtS3) ;
|
|
Frame3d PlDFrame ; PlDFrame.Set( ptD, vtD1, vtD2, vtD3) ;
|
|
Frame3d PlBFrame ; PlBFrame.Set( ptU, vtU1, vtU2, vtU3) ;
|
|
Frame3d PlTFrame ; PlTFrame.Set( ptTU, vtU1, vtU2, vtU3) ;
|
|
Frame3d PlIFrame ; PlIFrame.Set( ptU, vtW1, vtW2, vtW3) ;
|
|
Frame3d PlFFrame ; PlFFrame.Set( ptFU, vtW1, vtW2, vtW3) ;
|
|
Frame3d LargeEllipse ; LargeEllipse.Set( ptI, vtV1, vtV2, vtV3) ;
|
|
Frame3d FLargeEllipse ; FLargeEllipse.Set( ptF, vtV1, vtV2, vtV3) ;
|
|
Frame3d SmallEllipse ; SmallEllipse.Set( ptIT, vtV1, vtV2, vtV3) ;
|
|
Frame3d FSmallEllipse ; FSmallEllipse.Set( ptFT, vtV1, vtV2, vtV3) ;
|
|
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ORIG ; Vector3d vtK = Z_AX ;
|
|
|
|
// Cono I
|
|
ptC.LocToLoc( m_LocalFrame, IConeFrame) ;
|
|
vtK.LocToLoc( m_LocalFrame, IConeFrame) ;
|
|
|
|
std::vector <double> vdIConeCoef(3);
|
|
std::vector <double> vdIConeRoots;
|
|
|
|
vdIConeCoef[0] = dTan * dTan * ptC.x * ptC.x - ptC.y * ptC.y - ptC.z * ptC.z ;
|
|
vdIConeCoef[1] = 2 * ( dTan * dTan * ptC.x * vtK.x - ptC.y * vtK.y - ptC.z * vtK.z) ;
|
|
vdIConeCoef[2] = dTan * dTan * vtK.x * vtK.x - vtK.y * vtK.y - vtK.z * vtK.z ;
|
|
|
|
int nIConeRoot = PolynomialRoots( 2, vdIConeCoef, vdIConeRoots) ;
|
|
|
|
if ( nIConeRoot == 1) {
|
|
|
|
Point3d ptR1 = ptC + vdIConeRoots[0] * vtK ;
|
|
|
|
if ( ptR1.x >= dl && ptR1.x < dL) {
|
|
|
|
ptR1.LocToLoc( IConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR1.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= 0 && ptR1.x < dl) {
|
|
|
|
dMin = min( ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( nIConeRoot == 2) {
|
|
|
|
Point3d ptR1 = ptC + vdIConeRoots[0] * vtK ;
|
|
Point3d ptR2 = ptC + vdIConeRoots[1] * vtK ;
|
|
|
|
if ( ptR1.x > ptR2.x) {
|
|
|
|
Point3d ptTemp = ptR1 ;
|
|
ptR1 = ptR2 ;
|
|
ptR2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptR1.x < 0 && ptR2.x > 0 && ptR2.x < dl) {
|
|
|
|
dMin = min( ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x < 0 && ptR2.x >= dl && ptR2.x < dL) {
|
|
|
|
ptR2.LocToLoc( IConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR2.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR2.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x > 0 && ptR1.x < dl && ptR2.x >= dl && ptR2.x < dL) {
|
|
|
|
ptR2.LocToLoc( IConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR2.z, ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR2.z, ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x > 0 && ptR1.x < dl && ptR2.x >= dL) {
|
|
|
|
dMin = min( ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= dl && ptR1.x < dL && ptR2.x < dL) {
|
|
|
|
ptR1.LocToLoc( IConeFrame, m_LocalFrame) ;
|
|
ptR2.LocToLoc( IConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ptR2.z) ;
|
|
dMax = max( ptR1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= dl && ptR1.x < dL && ptR2.x >= dL) {
|
|
|
|
ptR1.LocToLoc( IConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR1.z, ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
// Cono F
|
|
ptC.LocToLoc( IConeFrame, FConeFrame) ;
|
|
vtK.LocToLoc( IConeFrame, FConeFrame) ;
|
|
|
|
std::vector <double> vdFConeCoef(3);
|
|
std::vector <double> vdFConeRoots;
|
|
|
|
vdFConeCoef[0] = dTan * dTan * ptC.x * ptC.x - ptC.y * ptC.y - ptC.z * ptC.z ;
|
|
vdFConeCoef[1] = 2 * ( dTan * dTan * ptC.x * vtK.x - ptC.y * vtK.y - ptC.z * vtK.z) ;
|
|
vdFConeCoef[2] = dTan * dTan * vtK.x * vtK.x - vtK.y * vtK.y - vtK.z * vtK.z ;
|
|
|
|
int nFConeRoot = PolynomialRoots( 2, vdFConeCoef, vdFConeRoots) ;
|
|
|
|
if ( nFConeRoot == 1) {
|
|
|
|
Point3d ptR1 = ptC + vdFConeRoots[0] * vtK ;
|
|
|
|
if ( ptR1.x >= dl && ptR1.x < dL) {
|
|
|
|
ptR1.LocToLoc( FConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= 0 && ptR1.x < dl) {
|
|
|
|
dMin = min( ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( nFConeRoot == 2) {
|
|
|
|
Point3d ptR1 = ptC + vdFConeRoots[0] * vtK ;
|
|
Point3d ptR2 = ptC + vdFConeRoots[1] * vtK ;
|
|
|
|
if ( ptR1.x > ptR2.x) {
|
|
|
|
Point3d ptTemp = ptR1 ;
|
|
ptR1 = ptR2 ;
|
|
ptR2 = ptTemp ;
|
|
}
|
|
|
|
|
|
if ( ptR1.x < 0 && ptR2.x > 0 && ptR2.x < dl) {
|
|
|
|
dMin = min( ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x < 0 && ptR2.x >= dl && ptR2.x < dL) {
|
|
|
|
ptR2.LocToLoc( FConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR2.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR2.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x > 0 && ptR1.x < dl && ptR2.x >= dl && ptR2.x < dL) {
|
|
|
|
ptR2.LocToLoc( FConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR2.z, ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR2.z, ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x > 0 && ptR1.x < dl && ptR2.x >= dL) {
|
|
|
|
dMin = min( ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= dl && ptR1.x < dL && ptR2.x < dL) {
|
|
|
|
ptR1.LocToLoc( FConeFrame, m_LocalFrame) ;
|
|
ptR2.LocToLoc( FConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ptR2.z) ;
|
|
dMax = max( ptR1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= dl && ptR1.x < dL && ptR2.x >= dL) {
|
|
|
|
ptR1.LocToLoc( FConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
// Solido interno
|
|
|
|
ptC.LocToLoc( FConeFrame, m_LocalFrame) ;
|
|
vtK.LocToLoc( FConeFrame, m_LocalFrame) ;
|
|
|
|
Point3d ptInt1 = ptC + ( ( ( vtRIV - vtC) * vtS1) / ( vtK * vtS1)) * vtK ;
|
|
Point3d ptInt2 = ptC + ( ( ( vtRIV - vtC) * vtD1) / ( vtK * vtD1)) * vtK ;
|
|
Point3d ptInt3 = ptC + ( ( ( vtRU - vtC) * vtU1) / ( vtK * vtU1)) * vtK ;
|
|
Point3d ptInt4 = ptC + ( ( ( vtRUT - vtC) * vtU1) / ( vtK * vtU1)) * vtK ;
|
|
Point3d ptInt5 = ptC + ( ( ( vtRU - vtC) * vtW1) / ( vtK * vtW1)) * vtK ;
|
|
Point3d ptInt6 = ptC + ( ( ( vtRFU - vtC) * vtW1) / ( vtK * vtW1)) * vtK ;
|
|
|
|
ptInt1.LocToLoc( m_LocalFrame, PlSFrame) ;
|
|
ptInt2.LocToLoc( m_LocalFrame, PlDFrame) ;
|
|
ptInt3.LocToLoc( m_LocalFrame, PlBFrame) ;
|
|
ptInt4.LocToLoc( m_LocalFrame, PlTFrame) ;
|
|
ptInt5.LocToLoc( m_LocalFrame, PlIFrame) ;
|
|
ptInt6.LocToLoc( m_LocalFrame, PlFFrame) ;
|
|
|
|
double dLim1, dLim2 ;
|
|
bool bFlag = false ;
|
|
|
|
if ( ptInt1.z >= 0 && ptInt1.z <= dPLong &&
|
|
ptInt1.y >= - ptInt1.z * dPOrt / dPLong &&
|
|
ptInt1.y <= dLen - ptInt1.z * dPOrt / dPLong ) {
|
|
|
|
ptInt1.LocToLoc( PlSFrame, m_LocalFrame) ;
|
|
|
|
dLim1 = ptInt1.z ;
|
|
bFlag = true ;
|
|
}
|
|
|
|
if ( ptInt2.z >= - dPLong && ptInt2.z <= 0 &&
|
|
ptInt2.y >= ptInt2.z * dPOrt / dPLong &&
|
|
ptInt2.y <= dLen + ptInt2.z * dPOrt / dPLong) {
|
|
|
|
ptInt2.LocToLoc( PlDFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt2.z ;
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt2.z ;
|
|
}
|
|
|
|
if ( ptInt3.y >= 0 && ptInt3.y <= dLen &&
|
|
ptInt3.z > - dMaxRad * dSin &&
|
|
ptInt3.z < dMaxRad * dSin) {
|
|
|
|
ptInt3.LocToLoc( PlBFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt3.z ;
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt3.z ;
|
|
}
|
|
|
|
if ( ptInt4.y >= 0 && ptInt4.y <= dLen &&
|
|
ptInt4.z > - dMinRad * dSin &&
|
|
ptInt4.z < dMinRad * dSin) {
|
|
|
|
ptInt4.LocToLoc( PlTFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt4.z ;
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt4.z ;
|
|
}
|
|
|
|
if ( ptInt5.y >= 0 && ptInt5.y <= dPr2 &&
|
|
ptInt5.z > - dSin * dMaxRad + ptInt5.y * prova1 / dPr2 &&
|
|
ptInt5.z < dSin * dMaxRad - ptInt5.y * prova1 / dPr2) {
|
|
|
|
ptInt5.LocToLoc( PlIFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt5.z ;
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt5.z ;
|
|
}
|
|
|
|
if ( ptInt6.y >= 0 && ptInt6.y <= dPr2 &&
|
|
ptInt6.z > - dSin * dMaxRad + ptInt6.y * prova1 / dPr2 &&
|
|
ptInt6.z < dSin * dMaxRad - ptInt6.y * prova1 / dPr2) {
|
|
|
|
ptInt6.LocToLoc( PlFFrame, m_LocalFrame) ;
|
|
|
|
if ( bFlag == false) {
|
|
|
|
dLim1 = ptInt6.z ;
|
|
bFlag = true ;
|
|
}
|
|
else
|
|
|
|
dLim2 = ptInt6.z ;
|
|
}
|
|
|
|
if( bFlag == true) {
|
|
|
|
dMin = min( dLim1, dLim2) ;
|
|
dMax = max( dLim1, dLim2) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
|
|
// Traslazioni ellissi
|
|
|
|
ptC.LocToLoc( m_LocalFrame, LargeEllipse) ;
|
|
vtK.LocToLoc( m_LocalFrame, LargeEllipse) ;
|
|
|
|
std::vector <double> vdLargeCoef(3);
|
|
std::vector <double> vdLargeRoots;
|
|
|
|
vdLargeCoef[0] = dCoef * dCoef * ptC.x * ptC.x + ptC.y * ptC.y + ptC.z * ptC.z - 2 * dCoef * ptC.x * ptC.y - dMaxRad * dMaxRad ;
|
|
vdLargeCoef[1] = 2 * ( dCoef * dCoef * vtK.x * ptC.x + vtK.y * ptC.y + vtK.z * ptC.z - dCoef * ( vtK.x * ptC.y + vtK.y * ptC.x)) ;
|
|
vdLargeCoef[2] = dCoef * dCoef * vtK.x * vtK.x + vtK.y * vtK.y + vtK.z * vtK.z - 2 * dCoef * vtK.x * vtK.y ;
|
|
|
|
|
|
int nLRoot = PolynomialRoots( 2, vdLargeCoef, vdLargeRoots) ;
|
|
|
|
if ( nLRoot == 0) {
|
|
|
|
Point3d ptPi ; ptPi.x = dX ; ptPi.y = dY ; ptPi.z = ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
Point3d ptPf ; ptPf.x = dX ; ptPf.y = dY ; ptPf.z = ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
ptPi.LocToLoc( m_LocalFrame, LargeEllipse) ;
|
|
ptPf.LocToLoc( m_LocalFrame, FLargeEllipse) ;
|
|
|
|
if ( ptPi.y * ptPi.y + ptPi.z * ptPi.z < dMaxRad * dMaxRad &&
|
|
ptPf.y * ptPf.y + ptPf.z * ptPf.z < dMaxRad * dMaxRad) {
|
|
|
|
ptPi.LocToLoc( LargeEllipse, m_LocalFrame) ;
|
|
ptPf.LocToLoc( FLargeEllipse, m_LocalFrame) ;
|
|
|
|
dMin = min( ptPi.z, ptPf.z) ;
|
|
dMax = max( ptPi.z, ptPf.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( nLRoot == 2) {
|
|
|
|
Point3d ptInter1 = ptC + vdLargeRoots[0] * vtK ;
|
|
Point3d ptInter2 = ptC + vdLargeRoots[1] * vtK ;
|
|
|
|
|
|
if ( ptInter1.x > ptInter2.x) {
|
|
|
|
Point3d ptTemp = ptInter1 ;
|
|
ptInter1 = ptInter2 ;
|
|
ptInter2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptInter1.x > 0 && ptInter1.x < dLLong &&
|
|
ptInter2.x > dLLong) {
|
|
|
|
ptInter1.LocToLoc( LargeEllipse, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter1.z) ;
|
|
dMax = max( ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptInter1.x > 0 && ptInter2.x < dLLong) {
|
|
|
|
ptInter1.LocToLoc( LargeEllipse, m_LocalFrame) ;
|
|
ptInter2.LocToLoc( LargeEllipse, m_LocalFrame) ;
|
|
|
|
dMin = min( ptInter1.z, ptInter2.z) ;
|
|
dMax = max( ptInter1.z, ptInter2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptInter1.x < 0 && ptInter2.x > dLLong) {
|
|
|
|
dMin = min( ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptInter1.x < 0 && ptInter2.x > 0 && ptInter2.x < dLLong) {
|
|
|
|
ptInter2.LocToLoc( LargeEllipse, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter2.z) ;
|
|
dMax = max( ( dDotCI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
ptC.LocToLoc( LargeEllipse, SmallEllipse) ;
|
|
vtK.LocToLoc( LargeEllipse, SmallEllipse) ;
|
|
|
|
std::vector <double> vdSmallCoef(3);
|
|
std::vector <double> vdSmallRoots;
|
|
|
|
vdSmallCoef[0] = dCoef * dCoef * ptC.x * ptC.x + ptC.y * ptC.y + ptC.z * ptC.z - 2 * dCoef * ptC.x * ptC.y - dMinRad * dMinRad ;
|
|
vdSmallCoef[1] = 2 * ( dCoef * dCoef * vtK.x * ptC.x + vtK.y * ptC.y + vtK.z * ptC.z - dCoef * ( vtK.x * ptC.y + vtK.y * ptC.x)) ;
|
|
vdSmallCoef[2] = dCoef * dCoef * vtK.x * vtK.x + vtK.y * vtK.y + vtK.z * vtK.z - 2 * dCoef * vtK.x * vtK.y ;
|
|
|
|
int nSRoot = PolynomialRoots( 2, vdSmallCoef, vdSmallRoots) ;
|
|
|
|
if ( nSRoot == 0) {
|
|
|
|
Point3d ptPi ; ptPi.x = dX ; ptPi.y = dY ; ptPi.z = ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
Point3d ptPf ; ptPf.x = dX ; ptPf.y = dY ; ptPf.z = ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
ptPi.LocToLoc( m_LocalFrame, SmallEllipse) ;
|
|
ptPf.LocToLoc( m_LocalFrame, FSmallEllipse) ;
|
|
|
|
if ( ptPi.y * ptPi.y + ptPi.z * ptPi.z < dMinRad * dMinRad &&
|
|
ptPf.y * ptPf.y + ptPf.z * ptPf.z < dMinRad * dMinRad) {
|
|
|
|
ptPi.LocToLoc( SmallEllipse, m_LocalFrame) ;
|
|
ptPf.LocToLoc( FSmallEllipse, m_LocalFrame) ;
|
|
|
|
dMin = min( ptPi.z, ptPf.z) ;
|
|
dMax = max( ptPi.z, ptPf.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( nSRoot == 2) {
|
|
|
|
Point3d ptTInter1 = ptC + vdSmallRoots[0] * vtK ;
|
|
Point3d ptTInter2 = ptC + vdSmallRoots[1] * vtK ;
|
|
|
|
if ( ptTInter1.x > ptTInter2.x) {
|
|
|
|
Point3d ptTemp = ptTInter1 ;
|
|
ptTInter1 = ptTInter2 ;
|
|
ptTInter2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptTInter1.x > 0 && ptTInter1.x < dLLong &&
|
|
ptTInter2.x > dLLong) {
|
|
|
|
ptTInter1.LocToLoc( SmallEllipse, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptTInter1.z) ;
|
|
dMax = max( ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptTInter1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptTInter1.x > 0 && ptTInter2.x < dLLong) {
|
|
|
|
ptTInter1.LocToLoc( SmallEllipse, m_LocalFrame) ;
|
|
ptTInter2.LocToLoc( SmallEllipse, m_LocalFrame) ;
|
|
|
|
dMin = min( ptTInter1.z, ptTInter2.z) ;
|
|
dMax = max( ptTInter1.z, ptTInter2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptTInter1.x < 0 && ptTInter2.x > dLLong) {
|
|
|
|
dMin = min( ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotCFT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptTInter1.x < 0 && ptTInter2.x > 0 && ptTInter2.x < dLLong) {
|
|
|
|
ptTInter2.LocToLoc( SmallEllipse, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptTInter2.z) ;
|
|
dMax = max( ( dDotCIT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptTInter2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::MillConusAux( const Point3d& ptI, const Point3d& ptF, const Vector3d& vtV1, const Vector3d& vtV2, const Vector3d& vtV3,
|
|
unsigned int & nStI, unsigned int & nStJ, unsigned int & nEnI, unsigned int & nEnJ,
|
|
double dHei, double dMaxRad, double dMinRad, double dCoef)
|
|
{
|
|
double dDeltaR = dMaxRad - dMinRad ;
|
|
double dL = ( ( dMaxRad * dHei) / ( dDeltaR)) ;
|
|
double dTan = dDeltaR / dHei ;
|
|
double dl = dL - dHei ;
|
|
|
|
double dLLong = abs( ( ptF - ptI) * vtV1) ;
|
|
double dLOrt = abs( ( ptF - ptI) * vtV2) ;
|
|
|
|
|
|
Point3d ptV = ptI - vtV1 * dL ;
|
|
Point3d ptT = ptI - vtV1 * dHei ;
|
|
|
|
Frame3d ConeFrame ; ConeFrame.Set( ptV, vtV1, vtV2, vtV3) ;
|
|
Frame3d IEllipseFrame ; IEllipseFrame.Set( ptI, vtV1, vtV2, vtV3) ;
|
|
Frame3d FEllipseFrame ; FEllipseFrame.Set( ptF, vtV1, vtV2, vtV3) ;
|
|
|
|
Vector3d vtI = ptI - ORIG ; double dDotI = vtI * vtV1 ;
|
|
Vector3d vtT = ptT - ORIG ; double dDotT = vtT * vtV1 ;
|
|
Vector3d vtF = ptF - ORIG ; double dDotF = vtF * vtV1 ;
|
|
|
|
Vector3d vtK = Z_AX ;
|
|
Vector3d vtKC = vtK ; vtKC.LocToLoc( m_LocalFrame, ConeFrame) ;
|
|
Vector3d vtKE = vtK ; vtKE.LocToLoc( m_LocalFrame, IEllipseFrame) ;
|
|
|
|
double dMin, dMax ;
|
|
|
|
for ( unsigned int i = nStI ; i <= nEnI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStJ ; j <= nEnJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ORIG ;
|
|
|
|
// Cono
|
|
ptC.LocToLoc( m_LocalFrame, ConeFrame) ;
|
|
|
|
std::vector <double> vdConeCoef(3);
|
|
std::vector <double> vdConeRoots;
|
|
|
|
vdConeCoef[0] = dTan * dTan * ptC.x * ptC.x - ptC.y * ptC.y - ptC.z * ptC.z ;
|
|
vdConeCoef[1] = 2 * ( dTan * dTan * ptC.x * vtKC.x - ptC.y * vtKC.y - ptC.z * vtKC.z) ;
|
|
vdConeCoef[2] = dTan * dTan * vtKC.x * vtKC.x - vtKC.y * vtKC.y - vtKC.z * vtKC.z ;
|
|
|
|
int nConeRoot = PolynomialRoots( 2, vdConeCoef, vdConeRoots) ;
|
|
|
|
|
|
if ( nConeRoot == 1) {
|
|
|
|
Point3d ptR1 = ptC + vdConeRoots[0] * vtKC ;
|
|
|
|
if ( ptR1.x >= dl && ptR1.x < dL) {
|
|
|
|
ptR1.LocToLoc( ConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR1.z, ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= 0 && ptR1.x < dl) {
|
|
|
|
dMin = min( ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
else if ( nConeRoot == 2) {
|
|
|
|
Point3d ptR1 = ptC + vdConeRoots[0] * vtKC ;
|
|
Point3d ptR2 = ptC + vdConeRoots[1] * vtKC ;
|
|
|
|
if ( ptR1.x > ptR2.x) {
|
|
|
|
Point3d ptTemp = ptR1 ;
|
|
ptR1 = ptR2 ;
|
|
ptR2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptR1.x < 0 && ptR2.x > 0 && ptR2.x < dl) {
|
|
|
|
dMin = min( ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x < 0 && ptR2.x >= dl && ptR2.x < dL) {
|
|
|
|
ptR2.LocToLoc( ConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR2.z, ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR2.z, ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x > 0 && ptR1.x < dl && ptR2.x >= dl && ptR2.x < dL) {
|
|
|
|
ptR2.LocToLoc( ConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR2.z, ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR2.z, ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x > 0 && ptR1.x < dl && ptR2.x >= dL) {
|
|
|
|
dMin = min( ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotT - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= dl && ptR1.x < dL && ptR2.x < dL) {
|
|
|
|
ptR1.LocToLoc( ConeFrame, m_LocalFrame) ;
|
|
ptR2.LocToLoc( ConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ptR2.z) ;
|
|
dMax = max( ptR1.z, ptR2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptR1.x >= dl && ptR1.x < dL && ptR2.x >= dL) {
|
|
|
|
ptR1.LocToLoc( ConeFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptR1.z, ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ptR1.z, ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
|
|
// Tralsazione dell'ellisse
|
|
|
|
ptC.LocToLoc( ConeFrame, IEllipseFrame) ;
|
|
|
|
std::vector <double> vdEllipseCoef(3);
|
|
std::vector <double> vdEllipseRoots;
|
|
|
|
vdEllipseCoef[0] = dCoef * dCoef * ptC.x * ptC.x + ptC.y * ptC.y + ptC.z * ptC.z - 2 * dCoef * ptC.x * ptC.y - dMaxRad * dMaxRad ;
|
|
vdEllipseCoef[1] = 2 * ( dCoef * dCoef * vtKE.x * ptC.x + vtKE.y * ptC.y + vtKE.z * ptC.z - dCoef * ( vtKE.x * ptC.y + vtKE.y * ptC.x)) ;
|
|
vdEllipseCoef[2] = dCoef * dCoef * vtKE.x * vtKE.x + vtKE.y * vtKE.y + vtKE.z * vtKE.z - 2 * dCoef * vtKE.x * vtKE.y ;
|
|
|
|
|
|
int nEllipseRoot = PolynomialRoots( 2, vdEllipseCoef, vdEllipseRoots) ;
|
|
|
|
|
|
if ( nEllipseRoot == 0 || nEllipseRoot == 1) {
|
|
|
|
Point3d ptPi ; ptPi.x = dX ; ptPi.y = dY ; ptPi.z = ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
Point3d ptPf ; ptPf.x = dX ; ptPf.y = dY ; ptPf.z = ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z ;
|
|
|
|
ptPi.LocToLoc( m_LocalFrame, IEllipseFrame) ;
|
|
ptPf.LocToLoc( m_LocalFrame, FEllipseFrame) ;
|
|
|
|
if ( ptPi.y * ptPi.y + ptPi.z * ptPi.z < dMaxRad * dMaxRad &&
|
|
ptPf.y * ptPf.y + ptPf.z * ptPf.z < dMaxRad * dMaxRad) {
|
|
|
|
ptPi.LocToLoc( IEllipseFrame, m_LocalFrame) ;
|
|
ptPf.LocToLoc( FEllipseFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptPi.z, ptPf.z) ;
|
|
dMax = max( ptPi.z, ptPf.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
if ( nEllipseRoot == 2) {
|
|
|
|
Point3d ptInter1 = ptC + vdEllipseRoots[0] * vtKE ;
|
|
Point3d ptInter2 = ptC + vdEllipseRoots[1] * vtKE ;
|
|
|
|
|
|
if ( ptInter1.x > ptInter2.x) {
|
|
|
|
Point3d ptTemp = ptInter1 ;
|
|
ptInter1 = ptInter2 ;
|
|
ptInter2 = ptTemp ;
|
|
}
|
|
|
|
if ( ptInter1.x > 0 && ptInter1.x < dLLong &&
|
|
ptInter2.x > dLLong) {
|
|
|
|
ptInter1.LocToLoc( IEllipseFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter1.z) ;
|
|
dMax = max( ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptInter1.x > 0 && ptInter2.x < dLLong) {
|
|
|
|
ptInter1.LocToLoc( IEllipseFrame, m_LocalFrame) ;
|
|
ptInter2.LocToLoc( IEllipseFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ptInter1.z, ptInter2.z) ;
|
|
dMax = max( ptInter1.z, ptInter2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptInter1.x < 0 && ptInter2.x > dLLong) {
|
|
|
|
dMin = min( ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
dMax = max( ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ( dDotF - vtV1.x * dX - vtV1.y * dY) / vtV1.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
else if ( ptInter1.x < 0 && ptInter2.x > 0 && ptInter2.x < dLLong) {
|
|
|
|
ptInter2.LocToLoc( IEllipseFrame, m_LocalFrame) ;
|
|
|
|
dMin = min( ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter2.z) ;
|
|
dMax = max( ( dDotI - vtV1.x * dX - vtV1.y * dY) / vtV1.z, ptInter2.z) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
// Traslazioni
|
|
//----------------------------------------------------------------------------
|
|
bool
|
|
VolZmap::Ball( const Point3d& ptLs, const Point3d& ptLe, double dRad)
|
|
{
|
|
double dMin, dMax ;
|
|
unsigned int nStartI, nStartJ, nEndI, nEndJ ;
|
|
|
|
bool Control = BBoxComponent( ptLs, ptLe, V_NULL, V_NULL, nStartI, nStartJ, nEndI, nEndJ, dRad, 0, 0) ;
|
|
|
|
if ( ! Control)
|
|
return true ;
|
|
|
|
Point3d ptI = ( ptLs.z < ptLe.z ? ptLs : ptLe) ;
|
|
Point3d ptF = ( ptLs.z < ptLe.z ? ptLe : ptLs) ;
|
|
|
|
Point3d ptIxy( ptI.x, ptI.y, 0) ;
|
|
Point3d ptFxy( ptF.x, ptF.y, 0) ;
|
|
|
|
Vector3d vtMove = ptF - ptI ;
|
|
Vector3d vtMoveXY( vtMove.x, vtMove.y, 0) ;
|
|
|
|
double dVLen = abs( vtMove.z) ; // Verticale e planare rispetto ai dexel
|
|
double dPLen = vtMoveXY.LenXY() ;
|
|
double dLen = vtMove.Len() ;
|
|
|
|
double dR1 = dVLen / dLen ;
|
|
double dR2 = dPLen / dLen ;
|
|
|
|
double dZI = ptI.z ;
|
|
double dDeltaZ = ptF.z - ptI.z ;
|
|
|
|
|
|
if ( dPLen < EPS_SMALL) {
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtC = ptC - ptIxy ;
|
|
|
|
double dSqLen = vtC.SqLen() ;
|
|
|
|
if ( dSqLen < dRad * dRad) {
|
|
|
|
double dH = sqrt( dRad * dRad - dSqLen) ;
|
|
|
|
dMin = dZI - dH ;
|
|
dMax = dZI + dDeltaZ + dH ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
|
|
Vector3d vtV1 = vtMoveXY ; vtV1.Normalize() ;
|
|
Vector3d vtV2 = vtV1 ; vtV2.Rotate( Z_AX, 90) ;
|
|
|
|
for ( unsigned int i = nStartI ; i <= nEndI ; ++ i) {
|
|
|
|
for ( unsigned int j = nStartJ ; j <= nEndJ ; ++ j) {
|
|
|
|
double dX = ( i + 0.5) * m_dStep ; double dY = ( j + 0.5) * m_dStep ;
|
|
|
|
Point3d ptC( dX, dY, 0) ; Vector3d vtCI = ptC - ptIxy ; Vector3d vtCF = ptC - ptFxy ;
|
|
|
|
double dX1 = vtCI * vtV1 ; double dX2 = vtCI * vtV2 ;
|
|
|
|
double dISqDist = vtCI * vtCI ; double dFSqDist = vtCF * vtCF ;
|
|
|
|
if ( dISqDist < dRad * dRad || dFSqDist < dRad * dRad ||
|
|
( dX1 > 0 && dX1 < dPLen && dX2 * dX2 < dRad * dRad)) {
|
|
|
|
// Massimi
|
|
if ( dX1 < - dRad * dR1 * sqrt( 1 - ( dX2 * dX2) / ( dRad * dRad * dR2 * dR2)) &&
|
|
dISqDist < dRad * dRad)
|
|
|
|
dMax = dZI + sqrt( dRad * dRad - dISqDist) ;
|
|
|
|
else if ( dX1 >= - dRad * dR1 * sqrt( 1 - ( dX2 * dX2) / ( dRad * dRad)) &&
|
|
dX1 < dPLen - dRad * dR1 * sqrt( 1 - ( dX2 * dX2) / ( dRad * dRad)))
|
|
|
|
dMax = dZI + dR2 * sqrt( dRad * dRad - dX2 * dX2) + ( dX1 + dRad * dR1 * sqrt( 1 - ( dX2 * dX2) / ( dRad * dRad))) * dVLen / dPLen ;
|
|
|
|
else if ( dX1 >= dPLen - dRad * dR1 * sqrt( 1 - ( dX2 * dX2) / ( dRad * dRad)) &&
|
|
dFSqDist < dRad * dRad)
|
|
|
|
dMax = dZI + dDeltaZ + sqrt( dRad * dRad - dFSqDist) ;
|
|
|
|
// Minimi
|
|
if ( dX1 < dRad * dR1 * sqrt( 1 - ( dX2 * dX2) / ( dRad * dRad)) &&
|
|
dISqDist < dRad * dRad)
|
|
|
|
dMin = dZI - sqrt( dRad * dRad - dISqDist) ;
|
|
|
|
else if ( dX1 >= dRad * dR1 * sqrt( 1 - ( dX2 * dX2) / ( dRad * dRad)) &&
|
|
dX1 < dPLen + dRad * dR1 * sqrt( 1 - ( dX2 * dX2) / ( dRad * dRad)))
|
|
|
|
dMin = dZI - dR2 * sqrt( dRad * dRad - dX2 * dX2) + ( dX1 - dRad * dR1 * sqrt( 1 - ( dX2 * dX2) / ( dRad * dRad))) * dVLen / dPLen ;
|
|
|
|
else if ( dX1 >= dPLen + dRad * dR1 * sqrt( 1 - ( dX2 * dX2) / ( dRad * dRad)) &&
|
|
dFSqDist < dRad * dRad)
|
|
|
|
dMin = dZI + dDeltaZ - sqrt( dRad * dRad - dFSqDist) ;
|
|
|
|
SubtractIntervals( i, j, dMin, dMax) ;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true ;
|
|
}
|
|
|
|
|
|
// Bounding Box, interferenza dell'utensile con lo Zmap e limiti su indici
|
|
//----------------------------------------------------------------------------
|
|
inline bool
|
|
VolZmap::BoundingBox( const Point3d& ptP1, const Point3d& ptP2, const Vector3d& vtV1, const Vector3d& vtV2,
|
|
unsigned int & nStI, unsigned int & nStJ, unsigned int & nEnI, unsigned int & nEnJ)
|
|
{
|
|
// Determinazione del raggio massimo dell'utensile
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
|
|
// Determinazione delle posizioni della punta dell'utensile nelle posizioni iniziale e finale
|
|
Point3d ptP1T = ptP1 - m_dHeight * vtV1 ;
|
|
Point3d ptP2T = ptP2 - m_dHeight * vtV2 ;
|
|
|
|
// Determinazione dei limiti del più piccolo parallelepipedo contenente il movimento
|
|
double dMinX = min( min( ptP1.x, ptP1T.x), min( ptP2.x, ptP2T.x)) - dMaxRad;
|
|
double dMinY = min( min( ptP1.y, ptP1T.y), min( ptP2.y, ptP2T.y)) - dMaxRad;
|
|
double dMinZ = min( min( ptP1.z, ptP1T.z), min( ptP2.z, ptP2T.z)) - dMaxRad;
|
|
double dMaxX = max( max( ptP1.x, ptP1T.x), max( ptP2.x, ptP2T.x)) + dMaxRad;
|
|
double dMaxY = max( max( ptP1.y, ptP1T.y), max( ptP2.y, ptP2T.y)) + dMaxRad;
|
|
double dMaxZ = max( max( ptP1.z, ptP1T.z), max( ptP2.z, ptP2T.z)) + dMaxRad;
|
|
|
|
// Verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return false ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return false ;
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return false ;
|
|
|
|
// Limiti su indici
|
|
nStI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
nEnI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
nStJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
nEnJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
inline bool
|
|
VolZmap::BoundingBox( unsigned int nGrid, const Point3d& ptP1, const Point3d& ptP2,
|
|
const Vector3d& vtV1, const Vector3d& vtV2,
|
|
unsigned int& nStI, unsigned int& nStJ, unsigned int& nEnI, unsigned int& nEnJ)
|
|
{
|
|
// NB: E' vitale che vengano passati i punti e i vettori nel sistema di riferimento
|
|
// di riferimento opportuno.
|
|
unsigned int nMaxNx, nMaxNy ;
|
|
|
|
double dMaxXValue, dMaxYValue ;
|
|
double dMinZValue, dMaxZValue ;
|
|
|
|
if ( nGrid == 1) {
|
|
|
|
nMaxNx = m_nNx ; nMaxNy = m_nNy ;
|
|
|
|
dMaxXValue = m_nNx * m_dStep ; dMaxYValue = m_nNy * m_dStep ;
|
|
|
|
dMinZValue = m_dMinZ ; dMaxZValue = m_dMaxZ ;
|
|
}
|
|
else if ( nGrid == 2) {
|
|
|
|
nMaxNx = m_nNx2 ; nMaxNy = m_nNy2 ;
|
|
|
|
dMaxXValue = m_nNx2 * m_dStep ; dMaxYValue = m_nNy2 * m_dStep ;
|
|
|
|
dMinZValue = m_dMinZ2 ; dMaxZValue = m_dMaxZ2 ;
|
|
}
|
|
else if ( nGrid == 3) {
|
|
|
|
nMaxNx = m_nNx3 ; nMaxNy = m_nNy3 ;
|
|
|
|
dMaxXValue = m_nNx3 * m_dStep ; dMaxYValue = m_nNy3 * m_dStep ;
|
|
|
|
dMinZValue = m_dMinZ3 ; dMaxZValue = m_dMaxZ3 ;
|
|
}
|
|
|
|
// Determinazione del raggio massimo dell'utensile
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
|
|
// Determinazione delle posizioni della punta dell'utensile nelle posizioni iniziale e finale
|
|
Point3d ptP1T = ptP1 - m_dHeight * vtV1 ;
|
|
Point3d ptP2T = ptP2 - m_dHeight * vtV2 ;
|
|
|
|
// Determinazione dei limiti del più piccolo parallelepipedo contenente il movimento
|
|
double dMinX = min( min( ptP1.x, ptP1T.x), min( ptP2.x, ptP2T.x)) - dMaxRad ;
|
|
double dMinY = min( min( ptP1.y, ptP1T.y), min( ptP2.y, ptP2T.y)) - dMaxRad ;
|
|
double dMinZ = min( min( ptP1.z, ptP1T.z), min( ptP2.z, ptP2T.z)) - dMaxRad ;
|
|
double dMaxX = max( max( ptP1.x, ptP1T.x), max( ptP2.x, ptP2T.x)) + dMaxRad ;
|
|
double dMaxY = max( max( ptP1.y, ptP1T.y), max( ptP2.y, ptP2T.y)) + dMaxRad ;
|
|
double dMaxZ = max( max( ptP1.z, ptP1T.z), max( ptP2.z, ptP2T.z)) + dMaxRad ;
|
|
|
|
// Verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > dMaxXValue - EPS_SMALL)
|
|
return false ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > dMaxYValue - EPS_SMALL)
|
|
return false ;
|
|
if ( dMaxZ < dMinZValue + EPS_SMALL || dMinZ > dMaxZValue - EPS_SMALL)
|
|
return false ;
|
|
|
|
// Limiti su indici
|
|
nStI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
nEnI = ( dMaxX > dMaxXValue - EPS_SMALL ? nMaxNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
nStJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
nEnJ = ( dMaxY > dMaxYValue - EPS_SMALL ? nMaxNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
inline bool
|
|
VolZmap::BoundingBox( const Point3d& ptP1, const Point3d& ptP2, const Vector3d& vtV1, const Vector3d& vtV2)
|
|
{
|
|
// Determinazione del raggio massimo dell'utensile
|
|
double dMaxRad = max( m_dRadius, m_dTipRadius) ;
|
|
|
|
// Determinazione delle posizioni della punta dell'utensile nelle posizioni iniziale e finale
|
|
Point3d ptP1T = ptP1 - m_dHeight * vtV1 ;
|
|
Point3d ptP2T = ptP2 - m_dHeight * vtV2 ;
|
|
|
|
// Determinazione dei limiti del più piccolo parallelepipedo contenente il movimento
|
|
double dMinX = min( min( ptP1.x, ptP1T.x), min( ptP2.x, ptP2T.x)) - dMaxRad ;
|
|
double dMinY = min( min( ptP1.y, ptP1T.y), min( ptP2.y, ptP2T.y)) - dMaxRad ;
|
|
double dMinZ = min( min( ptP1.z, ptP1T.z), min( ptP2.z, ptP2T.z)) - dMaxRad ;
|
|
double dMaxX = max( max( ptP1.x, ptP1T.x), max( ptP2.x, ptP2T.x)) + dMaxRad ;
|
|
double dMaxY = max( max( ptP1.y, ptP1T.y), max( ptP2.y, ptP2T.y)) + dMaxRad ;
|
|
double dMaxZ = max( max( ptP1.z, ptP1T.z), max( ptP2.z, ptP2T.z)) + dMaxRad ;
|
|
|
|
// Verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return false ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return false ;
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return false ;
|
|
|
|
return true ;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
inline bool
|
|
VolZmap::BBoxComponent( const Point3d& ptP1, const Point3d& ptP2, const Vector3d& vtV1, const Vector3d& vtV2,
|
|
unsigned int & nStI, unsigned int & nStJ, unsigned int & nEnI, unsigned int & nEnJ,
|
|
double dRad, double dTipRad, double dHei)
|
|
{
|
|
// Determinazione del raggio massimo dell'utensile
|
|
double dMaxRad = max( dRad, dTipRad) ;
|
|
|
|
// Determinazione delle posizioni della punta dell'utensile nelle posizioni iniziale e finale
|
|
Point3d ptP1T = ptP1 - dHei * vtV1 ;
|
|
Point3d ptP2T = ptP2 - dHei * vtV2 ;
|
|
|
|
// Determinazione dei limiti del più piccolo parallelepipedo contenente il movimento
|
|
double dMinX = min( min( ptP1.x, ptP1T.x), min( ptP2.x, ptP2T.x)) - dMaxRad;
|
|
double dMinY = min( min( ptP1.y, ptP1T.y), min( ptP2.y, ptP2T.y)) - dMaxRad;
|
|
double dMinZ = min( min( ptP1.z, ptP1T.z), min( ptP2.z, ptP2T.z)) - dMaxRad;
|
|
double dMaxX = max( max( ptP1.x, ptP1T.x), max( ptP2.x, ptP2T.x)) + dMaxRad;
|
|
double dMaxY = max( max( ptP1.y, ptP1T.y), max( ptP2.y, ptP2T.y)) + dMaxRad;
|
|
double dMaxZ = max( max( ptP1.z, ptP1T.z), max( ptP2.z, ptP2T.z)) + dMaxRad;
|
|
|
|
// Verifica dell'interferenza dell'utensile con lo Zmap
|
|
if ( dMaxX < EPS_SMALL || dMinX > m_nNx * m_dStep - EPS_SMALL)
|
|
return false ;
|
|
if ( dMaxY < EPS_SMALL || dMinY > m_nNy * m_dStep - EPS_SMALL)
|
|
return false ;
|
|
if ( dMaxZ < m_dMinZ + EPS_SMALL || dMinZ > m_dMaxZ - EPS_SMALL)
|
|
return false ;
|
|
|
|
// Limiti su indici
|
|
nStI = ( dMinX < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinX / m_dStep)) ;
|
|
nEnI = ( dMaxX > m_nNx * m_dStep - EPS_SMALL ? m_nNx - 1 : static_cast <unsigned int> ( dMaxX / m_dStep)) ;
|
|
nStJ = ( dMinY < EPS_SMALL ? 0 : static_cast <unsigned int> ( dMinY / m_dStep)) ;
|
|
nEnJ = ( dMaxY > m_nNy * m_dStep - EPS_SMALL ? m_nNy - 1 : static_cast <unsigned int> ( dMaxY / m_dStep)) ;
|
|
|
|
return true ;
|
|
} |