Include/EGkQuaternion.h

//----------------------------------------------------------------------------
//  EgalTech 2024-2024
//----------------------------------------------------------------------------
// File : EGkQuaternion.h          Data : 13.04.243         Versione : 2.6d4
// Contenuto : Dichiarazione della classe Quaternion.
//
//
//
// Modifiche : 31.12.13 DS Creazione modulo.
//
//
//
//----------------------------------------------------------------------------

#pragma once

#include "/EgtDev/Include/EGkFrame3d.h"

//----------------------- Macro per import/export -----------------------------
#undef EGK_EXPORT
#if defined( I_AM_EGK)            // da definirsi solo nella DLL
   #define EGK_EXPORT __declspec( dllexport)
#else
   #define EGK_EXPORT __declspec( dllimport)
#endif

//-----------------------------------------------------------------------------
class EGK_EXPORT Quaternion
{
   public :
      Quaternion( double dW = 0, double dX = 0, double dY = 0, double dZ = 0) : w( dW), x( dX), y( dY), z( dZ) {}

   public :
      bool IsValid( void) const
             { return ( std::isfinite( w)  &&  std::isfinite( x)  &&  std::isfinite( y)  &&  std::isfinite( z)) ; }
      bool IsSmall( void) const
             { return ( ( w * w + x * x + y * y + z * z) < SQ_EPS_SMALL) ; }
      bool IsZero( void) const
             { return ( ( w * w + x * x + y * y + z * z) < SQ_EPS_ZERO) ; }
      bool IsUnit( void) const
             { return ( abs( w - 1) < EPS_ZERO  &&  ( x * x + y * y + z * z) < SQ_EPS_ZERO) ; }
      double SqLen( void) const
             { return ( w * w + x * x + y * y + z * z) ; }
      double Len( void) const ;
      bool Normalize( double dEps = EPS_SMALL) ;
      bool IsNormalized( void) const
             { return ( abs( 1.0 - ( w * w + x * x + y * y + z * z)) < ( 2 * EPS_ZERO)) ; }
      Quaternion& operator +=( const Quaternion& qtQ)
             { w += qtQ.w ; x += qtQ.x ; y += qtQ.y ; z += qtQ.z ; return *this ; }
      Quaternion& operator -=( const Quaternion& qtQ)
             { w -= qtQ.w ;  x -= qtQ.x ; y -= qtQ.y ; z -= qtQ.z ; return *this ; }
      Quaternion& operator *=( double dMul)
             { w *= dMul ; x *= dMul ; y *= dMul ; z *= dMul ; return *this ; }
      Quaternion& operator /=( double dDiv)
             { double dMul = 1 / dDiv ; w *= dMul ; x *= dMul ; y *= dMul ; z *= dMul ; return *this ; }

   public :
      union {
         struct {
            double w ;
            double x ;
            double y ;
            double z ;
         } ;
         double v[4] ;
      } ;
} ;

//----------------------------------------------------------------------------
// Quaternioni notevoli
//----------------------------------------------------------------------------
// Quaternione non valido
const Quaternion Q_INVALID( NAN, NAN, NAN, NAN) ;
// Quaternione unità
const Quaternion Q_UNIT( 1, 0, 0, 0) ;
// Quaternione nullo
const Quaternion Q_NULL( 0, 0, 0, 0) ;

//----------------------------------------------------------------------------
// Definizione da rotazione definita con asse e angolo e inverso
//----------------------------------------------------------------------------
EGK_EXPORT Quaternion FromAxisAngle( const Vector3d& vtAx, double dAngDeg) ;
EGK_EXPORT bool ToAxisAngle( const Quaternion& qtQ, Vector3d& vtAx, double& dAngDeg) ;

//----------------------------------------------------------------------------
// Definizione da parte rotazione di un sistema di riferimento e inverso
//----------------------------------------------------------------------------
EGK_EXPORT Quaternion FromFrame( const Frame3d& frRef) ;
EGK_EXPORT bool ToFrame( const Quaternion& qtQ, Frame3d& frRef) ;

//----------------------------------------------------------------------------
// Coniugato di un quaternione
//----------------------------------------------------------------------------
inline const Quaternion 
Conjugate( const Quaternion& qtQ)
{
   return ( Quaternion( qtQ.w, -qtQ.x, -qtQ.y, -qtQ.z)) ; 
}

//----------------------------------------------------------------------------
// Opposto di un quaternione
//----------------------------------------------------------------------------
inline const Quaternion 
operator-( const Quaternion& qtQ)
{
   return ( Quaternion( -qtQ.w, - qtQ.x, - qtQ.y, - qtQ.z)) ; 
}

//----------------------------------------------------------------------------
// Somma di due quaternioni
//----------------------------------------------------------------------------
inline const Quaternion 
operator+( const Quaternion& qtQ1, const Quaternion& qtQ2)
{
   return ( Quaternion( qtQ1.w + qtQ2.w, qtQ1.x + qtQ2.x, qtQ1.y + qtQ2.y, qtQ1.z + qtQ2.z)) ; 
}

//----------------------------------------------------------------------------
// Sottrazione di due quaternioni
//----------------------------------------------------------------------------
inline const Quaternion 
operator-( const Quaternion& qtQ1, const Quaternion& qtQ2)
{
   return ( Quaternion( qtQ1.w - qtQ2.w, qtQ1.x - qtQ2.x, qtQ1.y - qtQ2.y, qtQ1.z - qtQ2.z)) ; 
}

//----------------------------------------------------------------------------
//  Prodotto di un quaternione con uno scalare
//----------------------------------------------------------------------------
inline const Quaternion 
operator*( const Quaternion& qtQ, double dMul)
{
   return ( Quaternion( qtQ.w * dMul, qtQ.x * dMul, qtQ.y * dMul, qtQ.z * dMul)) ; 
}

//----------------------------------------------------------------------------
//  Prodotto di uno scalare con un quaternione
//----------------------------------------------------------------------------
inline const Quaternion 
operator*( double dMul, const Quaternion& qtQ)
{
   return ( Quaternion( qtQ.w * dMul, qtQ.x * dMul, qtQ.y * dMul, qtQ.z * dMul)) ; 
}

//----------------------------------------------------------------------------
//  Divisione con uno scalare
//----------------------------------------------------------------------------
inline const Quaternion 
operator/( const Quaternion& qtQ, double dDiv)
{
   double dMul = 1 / dDiv ;
   return ( Quaternion( qtQ.w * dMul, qtQ.x * dMul, qtQ.y * dMul, qtQ.z * dMul)) ; 
}

//----------------------------------------------------------------------------
// Prodotto scalare due quaternioni
//----------------------------------------------------------------------------
inline double 
Scalar( const Quaternion& qtQ1, const Quaternion& qtQ2)
{
   return ( qtQ1.w * qtQ2.w + qtQ1.x * qtQ2.x + qtQ2.y * qtQ2.y + qtQ1.z * qtQ2.z) ; 
}

//----------------------------------------------------------------------------
// Prodotto di due quaternioni
//----------------------------------------------------------------------------
inline const Quaternion
operator*( const Quaternion& qtQ1, const Quaternion& qtQ2)
{
   return ( Quaternion( qtQ1.w * qtQ2.w - qtQ1.x * qtQ2.x - qtQ1.y * qtQ2.y - qtQ1.z * qtQ2.z,
                        qtQ1.w * qtQ2.x + qtQ1.x * qtQ2.w + qtQ1.y * qtQ2.z - qtQ1.z * qtQ2.y,
                        qtQ1.w * qtQ2.y + qtQ1.y * qtQ2.w + qtQ1.z * qtQ2.x - qtQ1.x * qtQ2.z,
                        qtQ1.w * qtQ2.z + qtQ1.z * qtQ2.w + qtQ1.x * qtQ2.y - qtQ1.y * qtQ2.x)) ;
}