// @(#)root/mathcore:$Id$
// Authors: W. Brown, M. Fischler, L. Moneta    2005  

 /**********************************************************************
  *                                                                    *
  * Copyright (c) 2005 , LCG ROOT FNAL MathLib Team                    *
  *                                                                    *
  *                                                                    *
  **********************************************************************/

// Header file for multiplications of AxisAngle by a rotation
//
// Created by: Mark Fischler Tues July 5  2005
//

#include "Math/GenVector/AxisAngle.h"

#include <cmath>

#include "Math/GenVector/AxisAngle.h"
#include "Math/GenVector/Quaternion.h"
#include "Math/GenVector/Cartesian3D.h"
#include "Math/GenVector/RotationX.h"
#include "Math/GenVector/RotationY.h"
#include "Math/GenVector/RotationZ.h"

#include "Math/GenVector/Rotation3Dfwd.h"
#include "Math/GenVector/EulerAnglesfwd.h"

namespace ROOT {

namespace Math {


AxisAngle AxisAngle::operator * (const Rotation3D & r) const {
   // combination with a Rotation3D
   return operator* ( Quaternion(r) );
   
}

AxisAngle AxisAngle::operator * (const AxisAngle   & a) const {
   // combination with another AxisAngle
   return operator* ( Quaternion(a) );
}

AxisAngle AxisAngle::operator * (const EulerAngles & e) const {
   // combination with EulerAngles
   return operator* ( Quaternion(e) );
}

AxisAngle AxisAngle::operator * (const RotationZYX & r) const {
   // combination with RotationZYX
   return operator* ( Quaternion(r) );
}

AxisAngle AxisAngle::operator * (const Quaternion  & q) const {
   // combination with Quaternion
   return AxisAngle ( Quaternion(*this) * q );
}

#ifdef MOVE

AxisAngle AxisAngle::operator * (const Quaternion  & q) const {
   // combination with quaternion (not used)
   const Scalar s1 = std::sin(fAngle/2);
   const Scalar au = std::cos(fAngle/2);
   const Scalar ai = s1 * fAxis.X();
   const Scalar aj = s1 * fAxis.Y();
   const Scalar ak = s1 * fAxis.Z();
   Scalar aqu = au*q.U() - ai*q.I() -  aj*q.J() -  ak*q.K();
   Scalar aqi = au*q.I() + ai*q.U() +  aj*q.K() -  ak*q.J();
   Scalar aqj = au*q.J() - ai*q.K() +  aj*q.U() +  ak*q.I();
   Scalar aqk = au*q.K() + ai*q.J() -  aj*q.I() +  ak*q.U();
   Scalar r = std::sqrt(aqi*aqi + aqj*aqj + aqk*aqk);
   if (r > 1) r = 1;
   if (aqu < 0) { aqu = - aqu; aqi = - aqi; aqj = - aqj, aqk = - aqk; }
   const Scalar angle = 2*asin ( r );
   DisplacementVector3D< Cartesian3D<double> > axis ( aqi, aqj, aqk );
   if ( r == 0 ) {
      axis.SetCoordinates(0,0,1);
   } else {
      axis /= r;
   }
   return AxisAngle ( axis, angle );
}

#endif

AxisAngle AxisAngle::operator * (const RotationX & rx) const {
   // combination with RotationX
   
   const Scalar s1 = std::sin(fAngle/2);
   const Scalar au = std::cos(fAngle/2);
   const Scalar ai = s1 * fAxis.X();
   const Scalar aj = s1 * fAxis.Y();
   const Scalar ak = s1 * fAxis.Z();
   Scalar c  = rx.CosAngle();
   if ( c > 1  ) c =  1;
   if ( c < -1 ) c = -1;
   Scalar qu = std::sqrt( .5*(1+c) );
   Scalar qi = std::sqrt( .5*(1-c) );
   if ( rx.SinAngle() < 0 ) qi = -qi;
   Scalar aqu = au*qu - ai*qi;
   Scalar aqi = ai*qu + au*qi;
   Scalar aqj = aj*qu + ak*qi;
   Scalar aqk = ak*qu - aj*qi;
   Scalar r = std::sqrt(aqi*aqi + aqj*aqj + aqk*aqk);
   if (r > 1) r = 1;
   if (aqu < 0) { aqu = - aqu; aqi = - aqi; aqj = - aqj, aqk = - aqk; }
   const Scalar angle = 2*asin ( r );
   DisplacementVector3D< Cartesian3D<double> > axis ( aqi, aqj, aqk );
   if ( r == 0 ) {
      axis.SetCoordinates(0,0,1);
   } else {
      axis /= r;
   }
   return AxisAngle ( axis, angle );
}

AxisAngle AxisAngle::operator * (const RotationY & ry) const {
   // combination with RotationY
   
   const Scalar s1 = std::sin(fAngle/2);
   const Scalar au = std::cos(fAngle/2);
   const Scalar ai = s1 * fAxis.X();
   const Scalar aj = s1 * fAxis.Y();
   const Scalar ak = s1 * fAxis.Z();
   Scalar c  = ry.CosAngle();
   if ( c > 1  ) c =  1;
   if ( c < -1 ) c = -1;
   Scalar qu = std::sqrt( .5*(1+c) );
   Scalar qj = std::sqrt( .5*(1-c) );
   if ( ry.SinAngle() < 0 ) qj = -qj;
   Scalar aqu = au*qu - aj*qj;
   Scalar aqi = ai*qu - ak*qj;
   Scalar aqj = aj*qu + au*qj;
   Scalar aqk = ak*qu + ai*qj;
   Scalar r = std::sqrt(aqi*aqi + aqj*aqj + aqk*aqk);
   if (r > 1) r = 1;
   if (aqu < 0) { aqu = - aqu; aqi = - aqi; aqj = - aqj, aqk = - aqk; }
   const Scalar angle = 2*asin ( r );
   DisplacementVector3D< Cartesian3D<double> > axis ( aqi, aqj, aqk );
   if ( r == 0 ) {
      axis.SetCoordinates(0,0,1);
   } else {
      axis /= r;
   }
   return AxisAngle ( axis, angle );
}

AxisAngle AxisAngle::operator * (const RotationZ & rz) const {
   // combination with RotationZ
   
   const Scalar s1 = std::sin(fAngle/2);
   const Scalar au = std::cos(fAngle/2);
   const Scalar ai = s1 * fAxis.X();
   const Scalar aj = s1 * fAxis.Y();
   const Scalar ak = s1 * fAxis.Z();
   Scalar c  = rz.CosAngle();
   if ( c > 1  ) c =  1;
   if ( c < -1 ) c = -1;
   Scalar qu = std::sqrt( .5*(1+c) );
   Scalar qk = std::sqrt( .5*(1-c) );
   if ( rz.SinAngle() < 0 ) qk = -qk;
   Scalar aqu = au*qu - ak*qk;
   Scalar aqi = ai*qu + aj*qk;
   Scalar aqj = aj*qu - ai*qk;
   Scalar aqk = ak*qu + au*qk;
   Scalar r = std::sqrt(aqi*aqi + aqj*aqj + aqk*aqk);
   if (r > 1) r = 1;
   if (aqu < 0) { aqu = - aqu; aqi = - aqi; aqj = - aqj, aqk = - aqk; }
   const Scalar angle = 2*asin ( r );
   DisplacementVector3D< Cartesian3D<double> > axis ( aqi, aqj, aqk );
   if ( r == 0 ) {
      axis.SetCoordinates(0,0,1);
   } else {
      axis /= r;
   }
   return AxisAngle ( axis, angle );
}

AxisAngle operator*( RotationX const & r, AxisAngle const & a ) {
   return AxisAngle(r) * a;  // TODO: improve performance
}

AxisAngle operator*( RotationY const & r, AxisAngle const & a ) {
   return AxisAngle(r) * a;  // TODO: improve performance
}

AxisAngle operator*( RotationZ const & r, AxisAngle const & a ) {
   return AxisAngle(r) * a;  // TODO: improve performance
}


} //namespace Math
} //namespace ROOT