EulerAngles.h

/*
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 without having a written signed agreement with 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.
*/
 
#ifndef MOMENTUM_EULERANGLES_H
#define MOMENTUM_EULERANGLES_H
 
#include <momentum_namespace.h>
#include <momentum_export.h>
 
#include "Vec3.h"
#include "Quat.h"
#include "Matrix4x4.h"
#include "EulerConvention.h"
#include <agx/EulerAngles.h>
 
namespace MOMENTUM_NAMESPACE
{
 
 
  class MOMENTUM_EXPORT EulerAngles
  {
 
    public:
 
 
#ifndef SWIG
#define MOMENTUM_EULER_AXES()  const int axes[4]  = {0,1,2,0}
#define MOMENTUM_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:
          const static char *names[]; // needed for debug purposes
          EulerOrder(EulerConvention::Convention = EulerConvention::DEFAULT_CONVENTION);
 
          int getConvention() const;
 
          bool operator=( const EulerConvention::Convention &c );
          bool operator=( const int &c );
          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 {
            MOMENTUM_EULER_AXES();
            return axes[firstAxis()];
          }
          inline int  j() const {
            MOMENTUM_EULER_PAXES();
            return paxes[i()+isOdd()];
          }
          inline int  k() const {
            MOMENTUM_EULER_PAXES();
            return paxes[i()+!isOdd()];
          }
      };
#endif
 
 
    private:
      double m_angles[3];
      EulerOrder m_o;
      void applyParity() ;
      void shiftxz();
      void flip();
      void permute();
      enum Axes {X, Y, Z, W};
 
    public:
 
      double x()const;
 
      double setX(double val);
 
      double y()const;
 
      double setY(double val);
 
      double z()const;
 
      double setZ(double val);
 
#ifndef SWIG
      inline double& operator [] (int i);
 
      inline double operator [] (int i) const;
#endif
      inline void set( const Vec3& angles );
 
      inline void set( double x,
                       double y,
                       double z );
 
 
      const EulerAngles::EulerOrder& setOrder(const EulerAngles::EulerOrder& order);
 
      const EulerAngles::EulerOrder& getOrder() const;
 
      //EulerAngles();
 
      EulerAngles(EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION);
 
#ifndef SWIG
      operator agx::EulerAngles() const
      {
        return agx::EulerAngles(x(), y(), z(), m_o.getConvention());
      }
 
      EulerAngles(const agx::EulerAngles& e)
      {
        (*this) = EulerAngles(e.x(), e.y(), e.z(), e.getOrder().getConvention());
      }
 
#endif
 
      EulerAngles( const EulerAngles & e );
 
#ifndef SWIG
      EulerAngles( double a, double b, double c, int C );
#endif
      EulerAngles(double a, double b, double c, EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION);
 
      explicit EulerAngles(const Vec3& angles, EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION);
 
 
#ifndef SWIG
      EulerAngles operator=( const EulerAngles& e );
#endif
      EulerAngles( const Matrix4x4 & m, EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION );
 
      EulerAngles( const Quat& q, EulerConvention::Convention C = EulerConvention::DEFAULT_CONVENTION );
 
 
      static EulerAngles random(const Vec3& min = Vec3(double(0.0)), const Vec3& max = Vec3(2 * MOMENTUM_NAMESPACE::PI), EulerConvention::Convention c = EulerConvention::DEFAULT_CONVENTION);
 
      void set(const Matrix4x4 & m, EulerConvention::Convention C = EulerConvention::BAD);
 
 
      void set(const Quat& q, EulerConvention::Convention C = EulerConvention::BAD);
 
      std::pair<Matrix4x4, bool> getAsMatrix4x4(Matrix4x4 matrix) const;
 
 
      std::pair<Quat, bool> getAsQuat() const;
 
      Vec3 asVec3() const;
 
  };
 
  inline int EulerAngles::EulerOrder::getConvention() const
  {
    return m_c;
  }
 
  inline bool EulerAngles::EulerOrder::operator=( const EulerConvention::Convention &c )
  {
    this->m_c = c;
    return true;
  }
 
  inline EulerAngles::EulerOrder::EulerOrder( EulerConvention::Convention c ) : m_c( c ) { }
 
  inline double EulerAngles::x() const
  {
    return m_angles[0];
  }
  inline double EulerAngles::setX(double val)
  {
    m_angles[0] = val;
    return m_angles[0];
  }
  inline double EulerAngles::y() const
  {
    return m_angles[1];
  }
  inline double EulerAngles::setY(double val)
  {
    m_angles[1] = val;
    return m_angles[1];
  }
  inline double EulerAngles::z() const
  {
    return m_angles[2];
  }
  inline double EulerAngles::setZ(double val)
  {
    m_angles[2] = val;
    return m_angles[2];
  }
 
  inline double& EulerAngles::operator [] (int i)
  {
    return m_angles[i];
  }
 
  inline double EulerAngles::operator [] (int i) const
  {
    return m_angles[i];
  }
 
 
  inline const EulerAngles::EulerOrder& EulerAngles::setOrder(const EulerAngles::EulerOrder& order)
  {
    m_o = order;
    return m_o;
  }
 
  inline const EulerAngles::EulerOrder& EulerAngles::getOrder() const
  {
    return m_o;
  }
 
  inline Vec3 EulerAngles::asVec3() const
  {
    return Vec3( m_angles[ 0 ], m_angles[ 1 ], m_angles[ 2 ] );
  }
 
  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( double x, double y, double 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;
  }
 
 
 
} // namespace momentum
 
#endif