//---------------------------------------------------------------------------- // EgalTech 2013-2013 //---------------------------------------------------------------------------- // File : Vector3d.cpp Data : 20.11.13 Versione : 1.3a1 // Contenuto : Funzioni della classe Vettore 3d. // // // // Modifiche : 04.01.13 DS Creazione modulo. // // //---------------------------------------------------------------------------- //--------------------------- Include ---------------------------------------- #include "stdafx.h" #include "\EgtDev\Include\EGkVector3d.h" #include "\EgtDev\Include\EGkFrame3d.h" //---------------------------------------------------------------------------- // Definizione a partire da coordinate sferiche //---------------------------------------------------------------------------- Vector3d FromSpherical( double dLen, double dAngVertDeg, double dAngOrizzDeg) { double dAngVertRad = dAngVertDeg * DEGTORAD ; double dAngOrizzRad = dAngOrizzDeg * DEGTORAD ; double dSinAngVert = sin( dAngVertRad) ; Vector3d vtV( dLen * dSinAngVert * cos( dAngOrizzRad), dLen * dSinAngVert * sin( dAngOrizzRad), dLen * cos( dAngVertRad)) ; return vtV ; } //---------------------------------------------------------------------------- // Definizione a partire da coordinate polari ( nel piano XY, Z = 0) //---------------------------------------------------------------------------- Vector3d FromPolar( double dLen, double dAngDeg) { double dAngRad = dAngDeg * DEGTORAD ; Vector3d vtV( dLen * cos( dAngRad), dLen * sin( dAngRad), 0) ; return vtV ; } //---------------------------------------------------------------------------- // Definizione dal più verticale dei vettori ortogonali a quello ricevuto //---------------------------------------------------------------------------- Vector3d FromUprightOrtho( const Vector3d& vtV) { // se vettore nullo, imposto asse Z+ if ( vtV.IsZero()) return Z_AX ; // se vettore coincidente con asse Z, imposto asse X+ if ( vtV.IsZplus() || vtV.IsZminus()) return X_AX ; // caso generico Vector3d vtAx = vtV ; vtAx.z = 0 ; vtAx.Normalize( EPS_ZERO) ; vtAx.Rotate( Z_AX, 0, 1) ; return ( vtV ^ vtAx) ; } //---------------------------------------------------------------------------- // Definizione dall'ortogonale al primo ricevuto, orizzontale e più vicino al secondo //---------------------------------------------------------------------------- Vector3d FromNearestHorizontalOrtho( const Vector3d& vtV, const Vector3d& vtNear) { // se vettore nullo o coincidente con asse Z, imposto asse orizzontale più vicino a Near if ( vtV.IsZero() || vtV.IsZplus() || vtV.IsZminus()) { Vector3d vtHor( vtNear.x, vtNear.y, 0) ; if ( vtHor.Normalize( EPS_ZERO)) return vtHor ; else return X_AX ; } // caso generico Vector3d vtHor = vtV ^ Z_AX ; vtHor.Normalize( EPS_ZERO) ; return ( vtHor * vtNear >= 0 ? vtHor : -vtHor) ; } //---------------------------------------------------------------------------- // Lunghezza del vettore //---------------------------------------------------------------------------- double Vector3d::Len( void) const { if ( abs( y) < EPS_ZERO && abs( z) < EPS_ZERO) return abs( x) ; if ( abs( z) < EPS_ZERO && abs( x) < EPS_ZERO) return abs( y) ; if ( abs( x) < EPS_ZERO && abs( y) < EPS_ZERO) return abs( z) ; return sqrt( x * x + y * y + z * z) ; } //---------------------------------------------------------------------------- // Lunghezza del vettore nel piano XY //---------------------------------------------------------------------------- double Vector3d::LenXY( void) const { if ( abs( y) < EPS_ZERO) return abs( x) ; if ( abs( x) < EPS_ZERO) return abs( y) ; return sqrt( x * x + y * y) ; } //---------------------------------------------------------------------------- // Ritorna la rappresentazione in coordinate sferiche //---------------------------------------------------------------------------- void Vector3d::ToSpherical( double* pdLen, double* pdAngVertDeg, double* pdAngOrizzDeg) const { // lunghezza double dLen = Len() ; // angoli double dAngVertDeg ; double dAngOrizzDeg ; // se vettore nullo if ( dLen < EPS_ZERO) { dAngVertDeg = 0 ; dAngOrizzDeg = 0 ; } // altrimenti else { // se diretto come Z if ( abs( x) < EPS_ZERO && abs( y) < EPS_ZERO) { dAngVertDeg = (( z > 0) ? 0 : ANG_STRAIGHT) ; dAngOrizzDeg = 0 ; } // se altrimenti nel piano XY else if ( abs( z) < EPS_ZERO) { dAngVertDeg = ANG_RIGHT ; dAngOrizzDeg = atan2( y, x) * RADTODEG ; if ( dAngOrizzDeg < 0) dAngOrizzDeg += ANG_FULL ; } // caso generico else { dAngVertDeg = acos( z / dLen) * RADTODEG ; dAngOrizzDeg = atan2( y, x) * RADTODEG ; if ( dAngOrizzDeg < 0) dAngOrizzDeg += ANG_FULL ; } } // ritorno i valori if ( pdLen != nullptr) *pdLen = dLen ; if ( pdAngVertDeg != nullptr) *pdAngVertDeg = dAngVertDeg ; if ( pdAngOrizzDeg != nullptr) *pdAngOrizzDeg = dAngOrizzDeg ; } //---------------------------------------------------------------------------- // Normalizzazione di un vettore (trasformazione in versore) //---------------------------------------------------------------------------- bool Vector3d::Normalize( double dEps) { // verifico validità if ( ! IsValid()) return false ; // se già normalizzato, ok double dSqLen = x * x + y * y + z * z ; if ( abs( 1.0 - dSqLen) < ( 2 * 1000 * DBL_EPSILON)) return true ; // se troppo piccolo, errore if ( dSqLen < ( dEps * dEps)) return false ; // eseguo la normalizzazione double dLen = sqrt( dSqLen) ; *this /= dLen ; return true ; } //---------------------------------------------------------------------------- // Rotazione //---------------------------------------------------------------------------- bool Vector3d::Rotate( const Vector3d& vtAx, double dAngDeg) { // se vettore nullo if ( IsZero()) return true ; // calcolo ed eseguo rotazione double dAngRad = dAngDeg * DEGTORAD ; return Rotate( vtAx, cos( dAngRad), sin( dAngRad)) ; } //---------------------------------------------------------------------------- // Rotazione da coseno e seno dell'angolo di rotazione //---------------------------------------------------------------------------- bool Vector3d::Rotate( const Vector3d& vtAx, double dCosAng, double dSinAng) { // se vettore nullo if ( IsZero()) return true ; // ricavo versore asse di rotazione Vector3d vtDirAx = vtAx ; if ( ! vtDirAx.Normalize()) return false ; // se rotazione attorno all'asse Z+ if ( vtAx.IsZplus()) { // salvo i componenti originali double dX = x ; double dY = y ; // calcolo i nuovi componenti ruotati x = dCosAng * dX - dSinAng * dY ; y = dSinAng * dX + dCosAng * dY ; return true ; } // se rotazione attorno all'asse Z- else if ( vtAx.IsZminus()) { // salvo i componenti originali double dX = x ; double dY = y ; // calcolo i nuovi componenti ruotati x = dCosAng * dX + dSinAng * dY ; y = - dSinAng * dX + dCosAng * dY ; return true ; } // se rotazione attorno all'asse X+ else if ( vtAx.IsXplus()) { // salvo i componenti originali double dY = y ; double dZ = z ; // calcolo i nuovi componenti ruotati y = dCosAng * dY - dSinAng * dZ ; z = dSinAng * dY + dCosAng * dZ ; return true ; } // se rotazione attorno all'asse X- else if ( vtAx.IsXminus()) { // salvo i componenti originali double dY = y ; double dZ = z ; // calcolo i nuovi componenti ruotati y = dCosAng * dY + dSinAng * dZ ; z = - dSinAng * dY + dCosAng * dZ ; return true ; } // se rotazione attorno all'asse Y+ else if ( vtAx.IsYplus()) { // salvo i componenti originali double dZ = z ; double dX = x ; // calcolo i nuovi componenti ruotati z = dCosAng * dZ - dSinAng * dX ; x = dSinAng * dZ + dCosAng * dX ; return true ; } // se rotazione attorno all'asse Y- else if ( vtAx.IsYminus()) { // salvo i componenti originali double dZ = z ; double dX = x ; // calcolo i nuovi componenti ruotati z = dCosAng * dZ + dSinAng * dX ; x = - dSinAng * dZ + dCosAng * dX ; return true ; } // rotazione attorno ad un asse generico // separazione del vettore nelle componenti parallela e perp. asse double dCompPar = *this * vtDirAx ; Vector3d vtCompPar = vtDirAx * dCompPar ; Vector3d vtCompPerp = *this - vtCompPar ; // calcolo vettore perp. componente perp. e asse Vector3d vtPerp2 = vtDirAx ^ vtCompPerp ; // calcolo componenti perp. del vettore ruotato Vector3d vtNewCompPerpX = dCosAng * vtCompPerp ; Vector3d vtNewCompPerpY = dSinAng * vtPerp2 ; Vector3d vtNewCompPerp = vtNewCompPerpX + vtNewCompPerpY ; // calcolo del vettore ruotato *this = vtCompPar + vtNewCompPerp ; return true ; } //---------------------------------------------------------------------------- // Scalatura non uniforme //---------------------------------------------------------------------------- bool Vector3d::Scale( const Frame3d& frRef, double dCoeffX, double dCoeffY, double dCoeffZ) { // verifico validità del frame if ( frRef.GetType() == Frame3d::ERR) return false ; // se vettore nullo if ( IsZero()) return true ; // ricavo le componenti sugli assi di scalatura, le scalo e riassemblo il vettore completo *this = ( (*this) * frRef.VersX() * dCoeffX) * frRef.VersX() + ( (*this) * frRef.VersY() * dCoeffY) * frRef.VersY() + ( (*this) * frRef.VersZ() * dCoeffZ) * frRef.VersZ() ; return true ; } //---------------------------------------------------------------------------- // Specchiatura //---------------------------------------------------------------------------- bool Vector3d::Mirror( const Vector3d& vtNorm) { // se vettore nullo if ( IsZero()) return true ; // ricavo versore normale al piano di simmetria Vector3d vtDirNorm = vtNorm ; if ( ! vtDirNorm.Normalize()) return false ; // calcolo la componente parallela alla normale double dCompNorm = *this * vtDirNorm ; // il simmetrico è il vettore originale meno il doppio della componente parallela *this = *this - 2 * dCompNorm * vtDirNorm ; return true ; } //---------------------------------------------------------------------------- // Scorrimento //---------------------------------------------------------------------------- bool Vector3d::Shear( const Vector3d& vtNorm, const Vector3d& vtDir, double dCoeff) { // se vettore nullo if ( IsZero()) return true ; // costruisco il vettore *this += dCoeff * ( *this * vtNorm) * vtDir ; return true ; } //---------------------------------------------------------------------------- // Cambio di riferimento : dal riferimento al globale //---------------------------------------------------------------------------- bool Vector3d::ToGlob( const Frame3d& frRef) { // verifico validità del frame if ( frRef.GetType() == Frame3d::ERR) return false ; // se vettore nullo o riferimento identità per le rotazioni if ( IsZero() || frRef.GetType() == Frame3d::TOP) return true ; // eseguo trasformazione Vector3d vtT( x, y, z) ; x = vtT.x * frRef.VersX().x + vtT.y * frRef.VersY().x + vtT.z * frRef.VersZ().x ; y = vtT.x * frRef.VersX().y + vtT.y * frRef.VersY().y + vtT.z * frRef.VersZ().y ; z = vtT.x * frRef.VersX().z + vtT.y * frRef.VersY().z + vtT.z * frRef.VersZ().z ; return true ; } //---------------------------------------------------------------------------- // Cambio di riferimento : dal globale al riferimento //---------------------------------------------------------------------------- bool Vector3d::ToLoc( const Frame3d& frRef) { // verifico validità del frame if ( frRef.GetType() == Frame3d::ERR) return false ; // se vettore nullo o riferimento identità per le rotazioni if ( IsZero() || frRef.GetType() == Frame3d::TOP) return true ; // eseguo trasformazione Vector3d vtT( x, y, z) ; x = vtT.x * frRef.VersX().x + vtT.y * frRef.VersX().y + vtT.z * frRef.VersX().z ; y = vtT.x * frRef.VersY().x + vtT.y * frRef.VersY().y + vtT.z * frRef.VersY().z ; z = vtT.x * frRef.VersZ().x + vtT.y * frRef.VersZ().y + vtT.z * frRef.VersZ().z ; return true ; } //---------------------------------------------------------------------------- // Cambio di riferimento : dal primo riferimento al secondo //---------------------------------------------------------------------------- bool Vector3d::LocToLoc( const Frame3d& frOri, const Frame3d& frDest) { // se vettore nullo o i due riferimenti coincidono, non devo fare alcunché if ( IsZero() || AreSameFrame( frOri, frDest)) return true ; return ( ToGlob( frOri) && ToLoc( frDest)) ; } //---------------------------------------------------------------------------- // Calcolo dell'angolo tra il vettore e un altro //---------------------------------------------------------------------------- bool Vector3d::GetAngle( const Vector3d& vtEnd, double& dAngDeg) const { // verifico vettori if ( IsZero() || vtEnd.IsZero()) { dAngDeg = 0 ; return false ; } // quantità ugualmente proporzionali a coseno e seno double dProSca = *this * vtEnd ; double dProVett = ( *this ^ vtEnd).Len() ; // se entrambe nulle if ( abs( dProSca) < EPS_ZERO && abs( dProVett) < EPS_ZERO) { dAngDeg = 0 ; return false ; } dAngDeg = atan2( dProVett, dProSca) * RADTODEG ; return true ; } //---------------------------------------------------------------------------- // Calcolo dell'angolo tra il vettore e un altro nel piano XY //---------------------------------------------------------------------------- bool Vector3d::GetAngleXY( const Vector3d& vtEnd, double& dAngDeg) const { // verifico vettori if ( IsZeroXY() || vtEnd.IsZeroXY()) { dAngDeg = 0 ; return false ; } // quantità ugualmente proporzionali a coseno e seno double dProSca = ScalarXY( *this, vtEnd) ; double dProVett = CrossXY( *this, vtEnd) ; // se entrambe nulle if ( abs( dProSca) < EPS_ZERO && abs( dProVett) < EPS_ZERO) { dAngDeg = 0 ; return false ; } dAngDeg = atan2( dProVett, dProSca) * RADTODEG ; return true ; } //---------------------------------------------------------------------------- // Calcolo angolo di rotazione per portare la componente del vettore perpendicolare // all'asse di rotazione sulla stessa direzione della componente perpendicolare di vtEnd //---------------------------------------------------------------------------- bool Vector3d::GetRotation( const Vector3d& vtEnd, const Vector3d& vtAx, double dEpsZero, double& dAngDeg, bool& bDet) const { // ricavo versore asse di rotazione Vector3d vtDirAx = vtAx ; if ( ! vtDirAx.Normalize()) return false ; // verifico epsilon per vettore nullo dEpsZero = std::min( EPS_SMALL, abs( dEpsZero)) ; // ricavo le componenti perpendicolari all'asse di rotazione Vector3d vtPerp = *this - vtDirAx * ( *this * vtDirAx) ; Vector3d vtPerpE = vtEnd - vtDirAx * ( vtEnd * vtDirAx) ; // se sono entrambe non nulle : angolo determinato if ( vtPerp.SqLen() > dEpsZero * dEpsZero && vtPerpE.SqLen() > dEpsZero * dEpsZero) { // secondo vettore perpendicolare ad asse e a componente perpendicolare Vector3d vtPerp2 = vtDirAx ^ vtPerp ; // proporzionali a seno e coseno del perpendicolare E double dKcosA = vtPerpE * vtPerp ; double dKsinA = vtPerpE * vtPerp2 ; // angolo di rotazione dAngDeg = atan2( dKsinA, dKcosA) * RADTODEG ; bDet = true ; return true ; } // se comunque sono entrambe molto piccole : angolo indeterminato else if ( vtPerp.IsSmall() && vtPerpE.IsSmall()) { dAngDeg = 0 ; bDet = false ; return true ; } // altrimenti angolo impossibile else { dAngDeg = 0 ; bDet = false ; return false ; } }