EulerAngles.h

Go to the documentation of this file.

/*
Copyright 2007-2025. Algoryx Simulation AB.
 
All AGX source code, intellectual property, documentation, sample code,
tutorials, scene files and technical white papers, are copyrighted, proprietary
and confidential material of Algoryx Simulation AB. You may not download, read,
store, distribute, publish, copy or otherwise disseminate, use or expose this
material unless having a written signed agreement with Algoryx Simulation AB, or having been
advised so by Algoryx Simulation AB for a time limited evaluation, or having purchased a
valid commercial license from Algoryx Simulation AB.
 
Algoryx Simulation AB disclaims all responsibilities for loss or damage caused
from using this software, unless otherwise stated in written agreements with
Algoryx Simulation AB.
*/
 
#pragma once
 
#include <agx/agxCore_export.h>
#include <agx/Matrix3x3.h>
#include <agx/agx.h>
#include <agx/Vec3.h>
#include <agx/Quat.h>
#include <agx/EulerConvention.h>
 
namespace agx
{
  class OrthoMatrix3x3;
 
  template <typename T>
  class Matrix3x3T;
 
  template <typename T>
  class Matrix4x4T;
 
  class AGXCORE_EXPORT EulerAngles
  {
    public:
 
#define AGX_EULER_AXES()  const int axes[4]  = {0,1,2,0}
#define AGX_EULER_PAXES() const int paxes[4] = {1,2,0,1};
 
      class EulerOrder
      {
        private:
          EulerConvention::Convention m_c;        // the current convention
        //  const static int axes[4];   // a utility for computations
        //  const static int paxes[4];  // a utility for computations
          // used internally
          inline int  firstAxis() const {
            return ( m_c >> 3 )&3;
          }
 
        public:
#ifndef SWIG
          const static char *names[]; // needed for debug purposes
#endif
          EulerOrder(EulerConvention::Convention = EulerConvention::DEFAULT_CONVENTION);
 
          int getConvention() const;
 
          bool operator=( const EulerConvention::Convention &c ); // NOLINT
 
          bool operator=( const int &c ); // NOLINT We should consider wether we really want to break convention here.
 
          inline bool isStatic() const  {
            return ! ( m_c&1 );
          }
          inline bool isOdd()    const  {
            return ( m_c >> 2 )&1;
          }
          inline bool isRepeat() const  {
            return ( m_c >> 1 )&1;
          }
          inline int  i() const {
            AGX_EULER_AXES();
            return axes[firstAxis()];
          }
          inline int  j() const {
            AGX_EULER_PAXES();
            return paxes[i()+isOdd()];
          }
          inline int  k() const {
            AGX_EULER_PAXES();
            return paxes[i()+!isOdd()];
          }
      };
 
 
    private:
      Real m_angles[3];
      EulerOrder m_o;
      void applyParity() ;
      void shiftxz();
      void flip();
      void permute();
      enum Axes {X, Y, Z, W};
 
    public:
 
      Real x()const;
 
      Real& x();
 
      Real y()const;
 
      Real& y();
 
      Real z()const;
 
      Real& z();
 
      inline Real& operator [] (size_t i);
 
      inline Real operator [] (size_t i) const;
 
      inline void set( const Vec3& angles );
 
      inline void set( Real x,
                       Real y,
                       Real z );
 
 
      EulerAngles::EulerOrder& getOrder();
 
      const EulerAngles::EulerOrder& getOrder() const;
 
      //EulerAngles();
 
      EulerAngles(EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION);
 
      //EulerAngles( EulerConvention::Convention C );
 
      EulerAngles( const EulerAngles & e ) = default;
 
      EulerAngles( Real a, Real b, Real c, int C );
 
      EulerAngles(Real a, Real b, Real c, EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION);
 
      EulerAngles & operator=( const EulerAngles& e ) = default;
 
      explicit EulerAngles(const agx::Vec3& angles, EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION);
 
      template <typename T>
      explicit EulerAngles( const Matrix4x4T<T> & m, EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION );
 
      template <typename T>
      explicit EulerAngles( const Matrix3x3T<T>& m, EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION );
      explicit EulerAngles(const OrthoMatrix3x3& m, EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION);
 
      explicit EulerAngles( const Quat& q, EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION );
 
      static EulerAngles random(const Vec3& min = Vec3(Real(0.0)), const Vec3& max = Vec3(2 * agx::PI), EulerConvention::Convention c = EulerConvention::DEFAULT_CONVENTION);
 
      //int setJacobianBody(Matrix3x3 & m);
      //int setJacobianWorld(Matrix3x3 & m);
 
      template<typename T>
      EulerAngles& set(const Matrix4x4T<T>& m, EulerConvention::Convention c = EulerConvention::BAD);
 
      template<typename T>
      EulerAngles& set(const Matrix3x3T<T> & m, EulerConvention::Convention C = EulerConvention::BAD);
      EulerAngles& set(const OrthoMatrix3x3 & m, EulerConvention::Convention C = EulerConvention::BAD);
 
      EulerAngles& set(const Quat& q, EulerConvention::Convention C = EulerConvention::BAD);
 
      template<typename T>
      bool get( Matrix4x4T<T>& m ) const;
 
      template<typename T>
      bool get( Matrix3x3T<T> & m ) const;
      bool get( OrthoMatrix3x3 & m ) const;
 
      bool get( Quat &q ) const;
 
      Real* ptr();
 
      const Real* ptr() const;
  };
 
  inline int EulerAngles::EulerOrder::getConvention() const
  {
    return m_c;
  }
 
  inline bool EulerAngles::EulerOrder::operator=( const EulerConvention::Convention &c ) // NOLINT
  {
    this->m_c = c;
    return true;
  }
 
  inline EulerAngles::EulerOrder::EulerOrder( EulerConvention::Convention c ) : m_c( c ) { }
 
  inline Real EulerAngles::x() const
  {
    return m_angles[0];
  }
 
  inline Real& EulerAngles::x()
  {
    return m_angles[0];
  }
 
  inline Real EulerAngles::y() const
  {
    return m_angles[1];
  }
 
  inline Real& EulerAngles::y()
  {
    return m_angles[1];
  }
 
  inline Real EulerAngles::z() const
  {
    return m_angles[2];
  }
 
  inline Real& EulerAngles::z()
  {
    return m_angles[2];
  }
 
  inline Real& EulerAngles::operator [] (size_t i)
  {
    return m_angles[i];
  }
 
  inline Real EulerAngles::operator [] (size_t i) const
  {
    return m_angles[i];
  }
 
 
  inline EulerAngles::EulerOrder& EulerAngles::getOrder()
  {
    return m_o;
  }
 
  inline const EulerAngles::EulerOrder& EulerAngles::getOrder() const
  {
    return m_o;
  }
 
 
  inline void EulerAngles::set( const Vec3& angles )
  {
    m_angles[0] = angles[0];
    m_angles[1] = angles[1];
    m_angles[2] = angles[2];
  }
 
  inline void EulerAngles::set( Real x, Real y, Real z )
  {
    m_angles[0] = x;
    m_angles[1] = y;
    m_angles[2] = z;
  }
 
 
  inline std::ostream& operator<< ( std::ostream& os, const EulerAngles& e )
  {
    os << "[" << e[0] << " " << e[1] << " " << e[2] << "]";
    return os;
  }
 
  AGX_FORCE_INLINE Real* EulerAngles::ptr()
  {
    return m_angles;
  }
 
  AGX_FORCE_INLINE const Real* EulerAngles::ptr() const
  {
    return m_angles;
  }
 
  //AGXCORE_EXPORT std::ostream & operator<< ( std::ostream &os, const EulerAngles & a );
  //AGXCORE_EXPORT std::ostream & operator<<( std::ostream &os, const EulerAngles::EulerOrder& o );
 
 
} // namespace agx