//----------------------------------------------------------------------------
//  EgalTech 2024-2024
//----------------------------------------------------------------------------
// File : Quaternion.cpp          Data : 13.04.24          Versione : 2.6d4
// Contenuto : Funzioni della classe Quaternion.
//
//
//
// Modifiche : 13.04.24 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------

//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "\EgtDev\Include\EGkQuaternion.h"

//----------------------------------------------------------------------------
// Lunghezza o Modulo
//----------------------------------------------------------------------------
double 
Quaternion::Len( void) const
{
   if ( abs( x) < EPS_ZERO  &&  abs( y) < EPS_ZERO  &&  abs( z) < EPS_ZERO)
      return abs( w) ;
   if ( abs( w) < EPS_ZERO  &&  abs( y) < EPS_ZERO  &&  abs( z) < EPS_ZERO)
      return abs( x) ;
   if ( abs( w) < EPS_ZERO  &&  abs( z) < EPS_ZERO  &&  abs( x) < EPS_ZERO)
      return abs( y) ;
   if ( abs( w) < EPS_ZERO  &&  abs( x) < EPS_ZERO  &&  abs( y) < EPS_ZERO)
      return abs( z) ;

   return sqrt( w * w + x * x + y * y + z * z) ; 
}

//----------------------------------------------------------------------------
// Normalizzazione
//----------------------------------------------------------------------------
bool
Quaternion::Normalize( double dEps)
{
  // se già normalizzato, ok
   double dSqLen = w * w + 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 ;
}

//----------------------------------------------------------------------------
Quaternion
FromAxisAngle( const Vector3d& vtAx, double dAngDeg)
{
   if ( abs( dAngDeg) < EPS_ANG_ZERO)
      return Q_UNIT ;
   double dLen = vtAx.Len() ;
   if ( dLen < EPS_ZERO)
      return Q_NULL ;
   double dDenom = 1 / dLen ;
   double dHCos = cos( dAngDeg / 2 * DEGTORAD) ;
   double dHSin = sin( dAngDeg / 2 * DEGTORAD) ;
   return Quaternion( dHCos, dHSin * vtAx.x * dDenom, dHSin * vtAx.y * dDenom, dHSin * vtAx.z * dDenom) ;
}

//----------------------------------------------------------------------------
bool
ToAxisAngle( const Quaternion& qtQ, Vector3d& vtAx, double& dAngDeg)
{
   if ( ! qtQ.IsNormalized())
      return false ;
   if ( qtQ.IsUnit()  ||  (-qtQ).IsUnit()) {
      dAngDeg = 0 ;
      vtAx = Z_AX ;
   }
   else {
      dAngDeg = 2 * acos( qtQ.w) * RADTODEG ;
      double dDenom = 1 / sqrt( 1 - qtQ.w * qtQ.w) ;
      vtAx.x = qtQ.x * dDenom ;
      vtAx.y = qtQ.y * dDenom ;
      vtAx.z = qtQ.z * dDenom ;
   }
   return true ;
}

//----------------------------------------------------------------------------
Quaternion
FromFrame( const Frame3d& frRef)
{
  // verifico il riferimento
   if ( ! frRef.IsValid())
      return Q_NULL ;
  // eseguo il calcolo
   Quaternion qtQ ;
  // traccia della matrice di rotazione
   double dTr = frRef.VersX().x + frRef.VersY().y + frRef.VersZ().z ;
  // se traccia positiva o nulla
   if ( dTr > - 10 * EPS_ZERO) {
      double dS = sqrt( 1 + dTr) ;
      qtQ.w = dS / 2 ;
      dS = 1 / ( 2 * dS) ;
      qtQ.x = ( frRef.VersY().z - frRef.VersZ().y) * dS ;
      qtQ.y = ( frRef.VersZ().x - frRef.VersX().z) * dS ;
      qtQ.z = ( frRef.VersX().y - frRef.VersY().x) * dS ;
   }
  // altrimenti traccia negativa
   else {
      if ( frRef.VersX().x > frRef.VersY().y - 10 * EPS_ZERO  &&  frRef.VersX().x > frRef.VersZ().z - 10 * EPS_ZERO) {
         double dS = sqrt( 1 + frRef.VersX().x - ( frRef.VersY().y + frRef.VersZ().z)) ;
         qtQ.x = dS / 2 ;
         dS = 1 / ( 2 * dS) ;
         qtQ.y = ( frRef.VersY().x + frRef.VersX().y) * dS ;
         qtQ.z = ( frRef.VersX().z + frRef.VersZ().x) * dS ;
         qtQ.w = ( frRef.VersY().z - frRef.VersZ().y) * dS ;
      }
      else if ( frRef.VersY().y > frRef.VersZ().z - 10 * EPS_ZERO  &&  frRef.VersY().y > frRef.VersX().x - 10 * EPS_ZERO) {
         double dS = sqrt( 1 + frRef.VersY().y - ( frRef.VersZ().z + frRef.VersX().x)) ;
         qtQ.y = dS / 2 ;
         dS = 1 / ( 2 * dS) ;
         qtQ.z = ( frRef.VersZ().y + frRef.VersY().z) * dS ;
         qtQ.x = ( frRef.VersY().x + frRef.VersX().y) * dS ;
         qtQ.w = ( frRef.VersZ().x - frRef.VersX().z) * dS ;
      }
      else {
         double dS = sqrt( 1 + frRef.VersZ().z - ( frRef.VersX().x + frRef.VersY().y)) ;
         qtQ.z = dS / 2 ;
         dS = 1 / ( 2 * dS) ;
         qtQ.x = ( frRef.VersX().z + frRef.VersZ().x) * dS ;
         qtQ.y = ( frRef.VersZ().y + frRef.VersY().z) * dS ;
         qtQ.w = ( frRef.VersX().y - frRef.VersY().x) * dS ;
      }
   }

   return qtQ ;
}

//----------------------------------------------------------------------------
bool
ToFrame( const Quaternion& qtQ, Frame3d& frRef)
{
   double dNrm = qtQ.Len() ;
   double dS = ( dNrm > EPS_ZERO ? 2 / dNrm : 0) ;
   double dXs = qtQ.x * dS, dYs = qtQ.y * dS, dZs = qtQ.z * dS ;
   double dWx = qtQ.w * dXs, dWy = qtQ.w * dYs, dWz = qtQ.w * dZs ;
   double dXx = qtQ.x * dXs, dXy = qtQ.x * dYs, dXz = qtQ.x * dZs ;
   double dYy = qtQ.y * dYs, dYz = qtQ.y * dZs, dZz = qtQ.z * dZs ;
   Vector3d vtX( 1 - ( dYy + dZz), dXy + dWz, dXz - dWy) ;
   Vector3d vtY( dXy - dWz, 1 - ( dXx + dZz), dYz + dWx) ;
   Vector3d vtZ( dXz + dWy, dYz - dWx, 1 - ( dXx + dYy)) ;
   return frRef.Set( ORIG, vtX, vtY, vtZ) ;
}