Dario Sassi
932700d4b2
- corretta AddRobotClimb (aggiungeva entità in posizione non corretta) - aggiunta gestione soluzione indeterminata con tre assi rotanti liberi.
105 lines
3.5 KiB
C++
105 lines
3.5 KiB
C++
//----------------------------------------------------------------------------
|
|
// EgalTech 2015-2015
|
|
//----------------------------------------------------------------------------
|
|
// File : GeoCalc.cpp Data : 12.05.15 Versione : 1.6e3
|
|
// Contenuto : Funzioni varie e speciali di calcolo geometrico.
|
|
//
|
|
//
|
|
//
|
|
// Modifiche : 12.05.15 DS Creazione modulo.
|
|
//
|
|
//
|
|
//----------------------------------------------------------------------------
|
|
|
|
//--------------------------- Include ----------------------------------------
|
|
#include "stdafx.h"
|
|
#include "GeoCalc.h"
|
|
|
|
//----------------------------------------------------------------------------
|
|
int
|
|
GetRotationComponent( const Vector3d& vtDir1, double dComp, const Vector3d& vtDir2, const Vector3d& vtRotAx,
|
|
double& dAng1Deg, double& dAng2Deg, bool& bDet)
|
|
{
|
|
// Vettori normalizzati.
|
|
// Si rimanda alle note, con vtDir1 == vtT, vtDir2 == vtZ, vtRotAx == vtW, dComp == dTaz.
|
|
|
|
// calcolo della componenete di T0 lungo W
|
|
double dT0w = vtDir1 * vtRotAx ;
|
|
|
|
// calcolo dell'asse U del riferimento UVW, coincidente con X di XYZ
|
|
Vector3d vtPvZW = vtDir2 ^ vtRotAx ;
|
|
|
|
// se Z e W sono allineati (o quasi)
|
|
Vector3d vtU = vtPvZW ;
|
|
if ( ! vtU.Normalize()) {
|
|
// determino se equiversi o controversi
|
|
bool bEquiv = ( vtDir2 * vtRotAx > 0) ;
|
|
// se le componenti concordano, angolo indeterminato
|
|
if ( abs( dT0w - ( bEquiv ? dComp : - dComp)) < 0.5 * SIN_EPS_ANG_SMALL) {
|
|
bDet = false ;
|
|
return 1 ;
|
|
}
|
|
// altrimenti, nessuna soluzione
|
|
else
|
|
return 0 ;
|
|
}
|
|
|
|
// calcolo della componenete di T0 lungo U
|
|
double dT0u = vtDir1 * vtU ;
|
|
|
|
// calcolo dell'asse V di UVW
|
|
Vector3d vtV = vtRotAx ^ vtU ;
|
|
|
|
// dovrebbe essere sempre normalizzabile, ma ...
|
|
if ( ! vtV.Normalize())
|
|
return 0 ;
|
|
|
|
// calcolo della componenete di T0 lungo V
|
|
double dT0v = vtDir1 * vtV ;
|
|
|
|
// calcolo coseno e seno dell'angolo comprezo tra vtZ e vtW, visto da vtU
|
|
double dCosG = vtDir2 * vtRotAx ;
|
|
double dSinG = vtPvZW * vtU ;
|
|
|
|
// calcolo della prima parte dell'angolo
|
|
double dOffsAngRad = PIGRECO / 2 - atan2( dT0v, dT0u) ;
|
|
|
|
// calcolo della seconda parte dell'angolo
|
|
// denominatore nullo ...
|
|
if ( ( abs( dT0v) < EPS_ZERO && abs( dT0u) < EPS_ZERO) || abs( dSinG) < SIN_EPS_ANG_ZERO) {
|
|
// calcolo della componenete di T0 lungo Z
|
|
double dT0z = vtDir1 * vtDir2 ;
|
|
// se le componenti concordano, angolo indeterminato
|
|
if ( abs( dT0z - dComp) < 0.5 * SIN_EPS_ANG_SMALL) {
|
|
bDet = false ;
|
|
return 1 ;
|
|
}
|
|
// altrimenti, nessuna soluzione
|
|
else
|
|
return 0 ;
|
|
}
|
|
double dT0uv = sqrt( dT0v * dT0v + dT0u * dT0u) ;
|
|
double dDenom = dT0uv * dSinG ;
|
|
double dNumer = dComp - dT0w * dCosG ;
|
|
// due angoli possibili
|
|
if ( abs( dDenom) > abs( dNumer) + SIN_EPS_ANG_ZERO) {
|
|
double dDeltaAngRad = acos( dNumer / dDenom) ;
|
|
dAng1Deg = ( dOffsAngRad + dDeltaAngRad) * RADTODEG ;
|
|
dAng2Deg = ( dOffsAngRad - dDeltaAngRad) * RADTODEG ;
|
|
bDet = true ;
|
|
return 2 ;
|
|
}
|
|
// un angolo possibile
|
|
else if ( abs( dDenom) > abs( dNumer) - SIN_EPS_ANG_ZERO) {
|
|
if ( dDenom * dNumer > 0)
|
|
dAng1Deg = dOffsAngRad * RADTODEG ;
|
|
else
|
|
dAng1Deg = dOffsAngRad * RADTODEG + ( dOffsAngRad < 0 ? ANG_STRAIGHT : - ANG_STRAIGHT) ;
|
|
bDet = true ;
|
|
return 1 ;
|
|
}
|
|
// nessun angolo possibile
|
|
else
|
|
return 0 ;
|
|
}
|