QuatTemplate.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.
*/
 
 
 
#ifndef AGX_QUATTEMPLATE_H
#define AGX_QUATTEMPLATE_H
 
#include <agx/agxCore_export.h>
 
#include <agx/agx.h>
#include <agx/Vec3.h>
#include <agx/Vec4.h>
#include <agx/EulerConvention.h>
 
#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 6385) // Disable warning C6385: Reading invalid data
#endif
 
 
namespace agx
{
  class EulerAngles;
  class OrthoMatrix3x3;
 
  template <typename T>
  class AffineMatrix4x4T;
 
  template <typename T>
  class QuatT
  {
    public:
 
      typedef T Type;
 
      QuatT();
 
      QuatT( T x, T y, T z, T w );
 
      explicit QuatT( const Vec4T<T>& v );
 
      QuatT( T angle, const Vec3T<T>& axis );
 
      QuatT( T angle1, const Vec3T<T>& axis1,
            T angle2, const Vec3T<T>& axis2,
            T angle3, const Vec3T<T>& axis3 );
 
      QuatT( const Vec3T<T>& from, const Vec3T<T>& to );
 
      explicit QuatT( const EulerAngles& euler);
 
 
      QuatT(const QuatT<T>& copy ) = default;
 
      template <typename T2>
      explicit QuatT(const QuatT<T2>& copy );
 
      explicit QuatT( const AffineMatrix4x4T<T>& matrix );
 
      QuatT<T>& operator = ( const QuatT<T>& v );
 
      bool operator == ( const QuatT<T>& v ) const;
 
      bool operator != ( const QuatT<T>& v ) const;
 
      /* ----------------------------------
      Methods to access data members
      ---------------------------------- */
 
      Vec4T<T> asVec4() const;
 
      Vec3T<T> asVec3() const;
 
      void set( T x, T y, T z, T w );
 
      void set( const Vec4T<T>& v );
 
      void get( EulerAngles& euler ) const;
 
      Vec3T<T> getAsEulerAngles(agx::EulerConvention::Convention convention = agx::EulerConvention::Convention::DEFAULT_CONVENTION) const;
 
      void get( AffineMatrix4x4T<T>& matrix ) const;
 
      void get(OrthoMatrix3x3& matrix) const;
 
      void get( T& angle, agx::Vec3T<T>& axis ) const;
 
 
      QuatT<T>& set( const EulerAngles& euler );
 
      QuatT<T>& set( const AffineMatrix4x4T<T>& matrix );
 
      QuatT<T>& set( const OrthoMatrix3x3& matrix );
 
 
      T& operator [] ( size_t i );
 
      T  operator [] ( size_t i ) const;
 
      T& x();
 
      T& y();
 
      T& z();
 
      T& w();
 
 
      T x() const;
 
      T y() const;
 
      T z() const;
 
      T w() const;
 
      bool zeroRotation() const;
 
 
      /* -------------------------------------------------------------
      BASIC ARITHMETIC METHODS
      Implemented in terms of Vec4s.  Some Vec4 operators, e.g.
      operator* are not appropriate for quaternions (as
      mathematical objects) so they are implemented differently.
      Also define methods for conjugate and the multiplicative inverse.
      ------------------------------------------------------------- */
      const QuatT<T> operator * ( T rhs ) const;
 
      QuatT<T>& operator *= ( T rhs );
 
      const QuatT<T> operator*( const QuatT<T>& rhs ) const;
 
      QuatT<T>& operator*=( const QuatT<T>& rhs );
 
      const QuatT<T> leftMult( const QuatT<T>& rhs ) const;
 
      const QuatT<T> rightMult( const QuatT<T>& rhs ) const;
 
 
      QuatT<T> operator / ( T rhs ) const;
 
      QuatT<T>& operator /= ( T rhs );
 
      const QuatT<T> operator/( const QuatT<T>& denom ) const;
 
      QuatT<T>& operator/=( const QuatT<T>& denom );
 
      const QuatT<T> operator + ( const QuatT<T>& rhs ) const;
 
      QuatT<T>& operator += ( const QuatT<T>& rhs );
 
      const QuatT<T> operator - ( const QuatT<T>& rhs ) const;
 
      QuatT<T>& operator -= ( const QuatT<T>& rhs );
 
      const QuatT<T> operator - () const;
 
      T length() const;
 
      T length2() const;
 
      QuatT<T> conj () const;
 
      QuatT<T> inverse () const;
 
      void setRotate( T angle, T x, T y, T z );
 
      void setRotate( T angle, const Vec3T<T>& vec );
 
      void setRotate ( T angle1, const Vec3T<T>& axis1,
                       T angle2, const Vec3T<T>& axis2,
                       T angle3, const Vec3T<T>& axis3 );
 
      void setRotate( const Vec3T<T>& from, const Vec3T<T>& to );
 
      static QuatT<T> rotate( T angle, T x, T y, T z );
 
      static QuatT<T> rotate( T angle, const Vec3T<T>& vec );
 
      static QuatT<T> rotate( T angle1, const Vec3T<T>& axis1,
                          T angle2, const Vec3T<T>& axis2,
                          T angle3, const Vec3T<T>& axis3 );
 
      static QuatT<T> rotate( const Vec3T<T>& from, const Vec3T<T>& to );
 
      void getRotate( T& angle, T& x, T& y, T& z ) const;
 
      void getRotate( T& angle, Vec3T<T>& vec ) const;
 
      agx::Real getAngle() const;
 
      agx::Vec3T<T> getUnitVector() const;
 
      void slerp( T t, const QuatT<T>& from, const QuatT<T>& to );
 
      Vec3T<T> operator* ( const Vec3T<T>& v ) const;
 
      T normalize();
 
      T* ptr();
 
      const T* ptr() const;
 
    protected:
      T m_data[4];
  };
 
 
  /* Implementation */
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>::QuatT()
  {
    m_data[0] = 0.0;
    m_data[1] = 0.0;
    m_data[2] = 0.0;
    m_data[3] = 1.0;
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>::QuatT( T x, T y, T z, T w )
  {
    m_data[0] = x;
    m_data[1] = y;
    m_data[2] = z;
    m_data[3] = w;
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>::QuatT( const Vec4T<T>& v )
  {
    m_data[0] = v.x();
    m_data[1] = v.y();
    m_data[2] = v.z();
    m_data[3] = v.w();
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>::QuatT( T angle, const Vec3T<T>& axis )
  {
    setRotate( angle, axis );
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>::QuatT( T angle1, const Vec3T<T>& axis1,
                               T angle2, const Vec3T<T>& axis2,
                               T angle3, const Vec3T<T>& axis3 )
  {
    setRotate( angle1, axis1, angle2, axis2, angle3, axis3 );
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>::QuatT( const Vec3T<T>& from, const Vec3T<T>& to )
  {
    setRotate( from, to );
  }
 
 
  template <typename T>
  template <typename T2>
  AGX_FORCE_INLINE QuatT<T>::QuatT(const QuatT<T2>& copy )
  {
    m_data[ 0 ] = (T)copy[ 0 ];
    m_data[ 1 ] = (T)copy[ 1 ];
    m_data[ 2 ] = (T)copy[ 2 ];
    m_data[ 3 ] = (T)copy[ 3 ];
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>::QuatT( const AffineMatrix4x4T<T>& matrix )
  {
    set( matrix );
  }
 
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>& QuatT<T>::operator = ( const QuatT<T>& v )
  {
    m_data[0] = v.m_data[0];
    m_data[1] = v.m_data[1];
    m_data[2] = v.m_data[2];
    m_data[3] = v.m_data[3];
    return *this;
  }
 
  template <typename T>
  AGX_FORCE_INLINE bool QuatT<T>::operator == ( const QuatT<T>& v ) const
  {
    return m_data[0] == v.m_data[0] && m_data[1] == v.m_data[1] && m_data[2] == v.m_data[2] && m_data[3] == v.m_data[3];
  }
 
  template <typename T>
  AGX_FORCE_INLINE bool QuatT<T>::operator != ( const QuatT<T>& v ) const
  {
    return m_data[0] != v.m_data[0] || m_data[1] != v.m_data[1] || m_data[2] != v.m_data[2] || m_data[3] != v.m_data[3];
  }
 
  template <typename T>
  AGX_FORCE_INLINE Vec4T<T> QuatT<T>::asVec4() const
  {
    return Vec4T<T>( m_data[0], m_data[1], m_data[2], m_data[3] );
  }
 
  template <typename T>
  AGX_FORCE_INLINE Vec3T<T> QuatT<T>::asVec3() const
  {
    return Vec3T<T>( m_data[0], m_data[1], m_data[2] );
  }
 
  template <typename T>
  AGX_FORCE_INLINE void QuatT<T>::set( T x, T y, T z, T w )
  {
    m_data[0] = x;
    m_data[1] = y;
    m_data[2] = z;
    m_data[3] = w;
  }
 
  template <typename T>
  AGX_FORCE_INLINE void QuatT<T>::set( const Vec4T<T>& v )
  {
    m_data[0] = v.x();
    m_data[1] = v.y();
    m_data[2] = v.z();
    m_data[3] = v.w();
  }
 
  template <typename T>
  AGX_FORCE_INLINE T& QuatT<T>::operator [] ( size_t i )
  {
    agxAssert( i < 4 );
    return m_data[i];
  }
 
  template <typename T>
  AGX_FORCE_INLINE T   QuatT<T>::operator [] ( size_t i ) const
  {
    agxAssert( i < 4 );
    return m_data[i];
  }
 
  template <typename T>
  AGX_FORCE_INLINE T& QuatT<T>::x()
  {
    return m_data[0];
  }
 
  template <typename T>
  AGX_FORCE_INLINE T& QuatT<T>::y()
  {
    return m_data[1];
  }
 
  template <typename T>
  AGX_FORCE_INLINE T& QuatT<T>::z()
  {
    return m_data[2];
  }
 
  template <typename T>
  AGX_FORCE_INLINE T& QuatT<T>::w()
  {
    return m_data[3];
  }
 
  template <typename T>
  AGX_FORCE_INLINE T QuatT<T>::x() const
  {
    return m_data[0];
  }
 
  template <typename T>
  AGX_FORCE_INLINE T QuatT<T>::y() const
  {
    return m_data[1];
  }
 
  template <typename T>
  AGX_FORCE_INLINE T QuatT<T>::z() const
  {
    return m_data[2];
  }
 
  template <typename T>
  AGX_FORCE_INLINE T QuatT<T>::w() const
  {
    return m_data[3];
  }
 
  template <typename T>
  inline bool QuatT<T>::zeroRotation() const
  {
    return m_data[0] == 0.0 && m_data[1] == 0.0 && m_data[2] == 0.0 && m_data[3] == 1.0;
  }
 
 
  template <typename T>
  AGX_FORCE_INLINE const QuatT<T> QuatT<T>::operator * ( T rhs ) const
  {
    return QuatT( m_data[0] * rhs, m_data[1] * rhs, m_data[2] * rhs, m_data[3] * rhs );
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>& QuatT<T>::operator *= ( T rhs )
  {
    m_data[0] *= rhs;
    m_data[1] *= rhs;
    m_data[2] *= rhs;
    m_data[3] *= rhs;
    return *this;        // enable nesting
  }
 
  template <typename T>
  AGX_FORCE_INLINE const QuatT<T> QuatT<T>::operator*( const QuatT<T>& rhs ) const
  {
    return QuatT( rhs.m_data[3] * m_data[0] + rhs.m_data[0] * m_data[3] + rhs.m_data[1] * m_data[2] - rhs.m_data[2] * m_data[1],
                 rhs.m_data[3] * m_data[1] - rhs.m_data[0] * m_data[2] + rhs.m_data[1] * m_data[3] + rhs.m_data[2] * m_data[0],
                 rhs.m_data[3] * m_data[2] + rhs.m_data[0] * m_data[1] - rhs.m_data[1] * m_data[0] + rhs.m_data[2] * m_data[3],
                 rhs.m_data[3] * m_data[3] - rhs.m_data[0] * m_data[0] - rhs.m_data[1] * m_data[1] - rhs.m_data[2] * m_data[2] );
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>& QuatT<T>::operator*=( const QuatT<T>& rhs )
  {
    T x = rhs.m_data[3] * m_data[0] + rhs.m_data[0] * m_data[3] + rhs.m_data[1] * m_data[2] - rhs.m_data[2] * m_data[1];
    T y = rhs.m_data[3] * m_data[1] - rhs.m_data[0] * m_data[2] + rhs.m_data[1] * m_data[3] + rhs.m_data[2] * m_data[0];
    T z = rhs.m_data[3] * m_data[2] + rhs.m_data[0] * m_data[1] - rhs.m_data[1] * m_data[0] + rhs.m_data[2] * m_data[3];
    m_data[3]   = rhs.m_data[3] * m_data[3] - rhs.m_data[0] * m_data[0] - rhs.m_data[1] * m_data[1] - rhs.m_data[2] * m_data[2];
 
    m_data[2] = z;
    m_data[1] = y;
    m_data[0] = x;
 
    return ( *this );          // enable nesting
  }
 
  template <typename T>
  AGX_FORCE_INLINE const QuatT<T> QuatT<T>::leftMult( const QuatT<T>& rhs ) const
  {
    return (*this) * rhs ;
  }
 
  template <typename T>
  AGX_FORCE_INLINE const QuatT<T> QuatT<T>::rightMult( const QuatT<T>& rhs ) const
  {
    return rhs * (*this) ;
  }
 
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T> QuatT<T>::operator / ( T rhs ) const
  {
    T div = T(1) / rhs;
    return QuatT( m_data[0] * div, m_data[1] * div, m_data[2] * div, m_data[3] * div );
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>& QuatT<T>::operator /= ( T rhs )
  {
    T div = T(1) / rhs;
    m_data[0] *= div;
    m_data[1] *= div;
    m_data[2] *= div;
    m_data[3] *= div;
    return *this;
  }
 
  template <typename T>
  AGX_FORCE_INLINE const QuatT<T> QuatT<T>::operator/( const QuatT<T>& denom ) const
  {
    return ( ( *this ) * denom.inverse() );
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>& QuatT<T>::operator/=( const QuatT<T>& denom )
  {
    ( *this ) = ( *this ) * denom.inverse();
    return ( *this );          // enable nesting
  }
 
  template <typename T>
  AGX_FORCE_INLINE const QuatT<T> QuatT<T>::operator + ( const QuatT<T>& rhs ) const
  {
    return QuatT( m_data[0] + rhs.m_data[0], m_data[1] + rhs.m_data[1],
                 m_data[2] + rhs.m_data[2], m_data[3] + rhs.m_data[3] );
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>& QuatT<T>::operator += ( const QuatT<T>& rhs )
  {
    m_data[0] += rhs.m_data[0];
    m_data[1] += rhs.m_data[1];
    m_data[2] += rhs.m_data[2];
    m_data[3] += rhs.m_data[3];
    return *this;            // enable nesting
  }
 
  template <typename T>
  AGX_FORCE_INLINE const QuatT<T> QuatT<T>::operator - ( const QuatT<T>& rhs ) const
  {
    return QuatT( m_data[0] - rhs.m_data[0], m_data[1] - rhs.m_data[1],
                 m_data[2] - rhs.m_data[2], m_data[3] - rhs.m_data[3] );
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T>& QuatT<T>::operator -= ( const QuatT<T>& rhs )
  {
    m_data[0] -= rhs.m_data[0];
    m_data[1] -= rhs.m_data[1];
    m_data[2] -= rhs.m_data[2];
    m_data[3] -= rhs.m_data[3];
    return *this;            // enable nesting
  }
 
  template <typename T>
  AGX_FORCE_INLINE const QuatT<T> QuatT<T>::operator - () const
  {
    return QuatT<T> ( -m_data[0], -m_data[1], -m_data[2], -m_data[3] );
  }
 
  template <typename T>
  AGX_FORCE_INLINE T QuatT<T>::length() const
  {
    return std::sqrt( m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2] + m_data[3] * m_data[3] );
  }
 
  template <typename T>
  AGX_FORCE_INLINE T QuatT<T>::length2() const
  {
    return m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2] + m_data[3] * m_data[3];
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T> QuatT<T>::conj () const
  {
    return QuatT( -m_data[0], -m_data[1], -m_data[2], m_data[3] );
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T> QuatT<T>::inverse () const
  {
    return conj() / length2();
  }
 
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T> QuatT<T>::rotate( T angle, T x, T y, T z )
  {
    return rotate( angle, Vec3T<T>( x, y, z ) );
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T> QuatT<T>::rotate( T angle, const Vec3T<T>& vec )
  {
    QuatT<T> q;
    q.setRotate( angle, vec );
    return q;
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T> QuatT<T>::rotate( T angle1, const Vec3T<T>& axis1,
                                      T angle2, const Vec3T<T>& axis2,
                                      T angle3, const Vec3T<T>& axis3 )
  {
    QuatT<T> q;
    q.setRotate( angle1, axis1, angle2, axis2, angle3, axis3 );
    return q;
  }
 
  template <typename T>
  AGX_FORCE_INLINE QuatT<T> QuatT<T>::rotate( const Vec3T<T>& from, const Vec3T<T>& to )
  {
    QuatT<T> q;
    q.setRotate( from, to );
    return q;
  }
 
 
  template <typename T>
  inline Vec3T<T> QuatT<T>::operator* ( const Vec3T<T>& v ) const
  {
    // nVidia SDK implementation
    Vec3T<T> uv, uuv;
    Vec3T<T> qvec( m_data[0], m_data[1], m_data[2] );
    uv = qvec ^ v;
    uuv = qvec ^ uv;
    uv *= ( 2.0f * m_data[3] );
    uuv *= 2.0f;
    return v + uv + uuv;
  }
 
  template <typename T>
  AGX_FORCE_INLINE T QuatT<T>::normalize()
  {
    T norm = this->length();
    if ( norm > 0.0 ) {
      T inv = 1.0f / norm;
      m_data[0] *= inv;
      m_data[1] *= inv;
      m_data[2] *= inv;
      m_data[3] *= inv;
    }
    return( norm );
  }
 
 
  template <typename T>
  AGX_FORCE_INLINE T* QuatT<T>::ptr()
  {
    return m_data;
  }
 
  template <typename T>
  AGX_FORCE_INLINE const T* QuatT<T>::ptr() const
  {
    return m_data;
  }
 
 
  template <typename T>
  AGX_FORCE_INLINE bool equivalent( const agx::QuatT<T>& a, const agx::QuatT<T>& b, T epsilon = T(AGX_EQUIVALENT_EPSILON))
  {
    return
      agx::equivalent(a[0], b[0], epsilon) &&
      agx::equivalent(a[1], b[1], epsilon) &&
      agx::equivalent(a[2], b[2], epsilon) &&
      agx::equivalent(a[3], b[3], epsilon);
  }
 
 
  template <typename T>
  inline std::ostream& operator << ( std::ostream& output, const QuatT<T>& quat )
  {
    output << quat[0] << " "
           << quat[1] << " "
           << quat[2] << " "
           << quat[3];
    return output;     // to enable cascading
  }
 
}
 
#ifdef _MSC_VER
# pragma warning(pop)
#endif
 
#endif /* AGX_QUATTEMPLATE_H */