Quat.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_QUAT_H
#define MOMENTUM_QUAT_H
 
#include "momentum_namespace.h"
#include "momentum_math.h"
 
#include "Vec3.h"
#include "Vec4.h"
#include "EulerConvention.h"
 
#include <agx/Quat.h>
#include <agx/agx.h>
 
#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 6385) // Disable warning C6385: Reading invalid data
#endif
 
 
namespace MOMENTUM_NAMESPACE
{
  class EulerAngles;
 
  class AffineMatrix4x4;
 
 
  class MOMENTUM_EXPORT Quat
  {
  public:
#ifndef SWIG
    operator agx::Quat() const
    {
      return agx::Quat(this->x(), this->y(), this->z(), this->w());
    }
 
    Quat(const agx::Quat q)
    {
      set(q.x(), q.y(), q.z(), q.w());
    }
 
#endif
 
 
    Quat();
 
    Quat(double x, double y, double z, double w);
 
    explicit Quat(const Vec4& v);
 
    Quat(double angle, const Vec3& axis);
 
    Quat(double angle1, const Vec3& axis1,
         double angle2, const Vec3& axis2,
         double angle3, const Vec3& axis3);
 
    Quat(const Vec3& from, const Vec3& to);
 
    explicit Quat(const EulerAngles& euler);
 
 
    Quat(const Quat& copy);
 
    explicit Quat(const AffineMatrix4x4& matrix);
 
#ifndef SWIG
    Quat operator = (const Quat& v);
#endif
    bool operator == (const Quat& v) const;
 
    bool operator != (const Quat& v) const;
 
    /* ----------------------------------
    Methods to access data members
    ---------------------------------- */
 
    Vec4 asVec4() const;
 
    Vec3 asVec3() const;
 
    void set(double x, double y, double z, double w);
 
    void set(const Vec4& v);
 
 
    Vec3 getAsEulerAngles(EulerConvention::Convention convention = EulerConvention::Convention::DEFAULT_CONVENTION) const;
 
    AffineMatrix4x4 get(const AffineMatrix4x4& matrix) const;
 
 
    std::pair<double,Vec3> getAsAngleAndVector() const;
 
 
    Quat set(const EulerAngles& euler);
 
    Quat set(const AffineMatrix4x4& matrix);
 
#ifndef SWIG
    double& operator [] (int i);
 
    double operator [] (int i) const;
#endif
    double setX(double val);
 
    double setY(double val);
 
    double setZ(double val);
 
    double setW(double val);
 
    double x() const;
 
    double y() const;
 
    double z() const;
 
    double 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 Quat operator * (double rhs) const;
 
    Quat operator *= (double rhs);
 
    const Quat operator*(const Quat& rhs) const;
 
    Quat operator*=(const Quat& rhs);
 
    const Quat leftMult(const Quat& rhs) const;
 
    const Quat rightMult(const Quat& rhs) const;
 
 
    Quat operator / (double rhs) const;
 
    Quat operator /= (double rhs);
 
    const Quat operator/(const Quat& denom) const;
 
    Quat operator/=(const Quat& denom);
 
    const Quat operator + (const Quat& rhs) const;
 
    Quat operator += (const Quat& rhs);
 
    const Quat operator - (const Quat& rhs) const;
 
    Quat operator -= (const Quat& rhs);
 
    const Quat operator - () const;
 
    double length() const;
 
    double length2() const;
 
    Quat conj() const;
 
    Quat inverse() const;
 
    void setRotate(double angle, double x, double y, double z);
 
    void setRotate(double angle, const Vec3& vec);
 
    void setRotate(double angle1, const Vec3& axis1,
      double angle2, const Vec3& axis2,
      double angle3, const Vec3& axis3);
 
    void setRotate(const Vec3& from, const Vec3& to);
 
    static Quat rotate(double angle, double x, double y, double z);
 
    static Quat rotate(double angle, const Vec3& vec);
 
    static Quat rotate(double angle1, const Vec3& axis1,
      double angle2, const Vec3& axis2,
      double angle3, const Vec3& axis3);
 
    static Quat rotate(const Vec3& from, const Vec3& to);
 
    std::pair<double, Vec3> getRotate() const;
 
    void slerp(double t, const Quat& from, const Quat& to);
 
    Vec3 operator* (const Vec3& v) const;
 
    double normalize();
 
  protected:
    double m_data[4];
  };
 
#ifndef SWIG
  /* Implementation */
  inline Quat::Quat()
  {
    m_data[0] = 0.0;
    m_data[1] = 0.0;
    m_data[2] = 0.0;
    m_data[3] = 1.0;
  }
 
  inline Quat::Quat(double x, double y, double z, double w)
  {
    m_data[0] = x;
    m_data[1] = y;
    m_data[2] = z;
    m_data[3] = w;
  }
 
  inline Quat::Quat(const Vec4& v)
  {
    m_data[0] = v.x();
    m_data[1] = v.y();
    m_data[2] = v.z();
    m_data[3] = v.w();
  }
 
  inline Quat::Quat(double angle, const Vec3& axis)
  {
    setRotate(angle, axis);
  }
 
  inline Quat::Quat(double angle1, const Vec3& axis1,
    double angle2, const Vec3& axis2,
    double angle3, const Vec3& axis3)
  {
    setRotate(angle1, axis1, angle2, axis2, angle3, axis3);
  }
 
  inline Quat::Quat(const Vec3& from, const Vec3& to)
  {
    setRotate(from, to);
  }
 
  inline Quat::Quat(const EulerAngles& euler)
  {
    set(euler);
  }
 
 
  inline Quat::Quat(const Quat& copy)
  {
    m_data[0] = copy.m_data[0];
    m_data[1] = copy.m_data[1];
    m_data[2] = copy.m_data[2];
    m_data[3] = copy.m_data[3];
  }
 
  inline Quat::Quat(const AffineMatrix4x4& matrix)
  {
    set(matrix);
  }
 
 
  inline Quat Quat::operator = (const Quat& 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;
  }
 
  inline bool Quat::operator == (const Quat& 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];
  }
 
  inline bool Quat::operator != (const Quat& 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];
  }
 
  inline Vec4 Quat::asVec4() const
  {
    return Vec4(m_data[0], m_data[1], m_data[2], m_data[3]);
  }
 
  inline Vec3 Quat::asVec3() const
  {
    return Vec3(m_data[0], m_data[1], m_data[2]);
  }
 
  inline void Quat::set(double x, double y, double z, double w)
  {
    m_data[0] = x;
    m_data[1] = y;
    m_data[2] = z;
    m_data[3] = w;
  }
 
  inline void Quat::set(const Vec4& v)
  {
    m_data[0] = v.x();
    m_data[1] = v.y();
    m_data[2] = v.z();
    m_data[3] = v.w();
  }
 
  inline double Quat::operator [] (int i) const
  {
    agxAssert(i < 4);
    return m_data[i];
  }
 
  inline double& Quat::operator [] (int i)
  {
    agxAssert(i < 4);
    return m_data[i];
  }
 
  inline double Quat::setX(double val)
  {
    m_data[0] = val;
    return m_data[0];
  }
 
  inline double Quat::setY(double val)
  {
    m_data[1] = val;
    return m_data[1];
  }
 
  inline double Quat::setZ(double val)
  {
    m_data[2] = val;
    return m_data[2];
  }
 
  inline double Quat::setW(double val)
  {
    m_data[3] = val;
    return m_data[3];
  }
 
  inline double Quat::x() const
  {
    return m_data[0];
  }
 
  inline double Quat::y() const
  {
    return m_data[1];
  }
 
  inline double Quat::z() const
  {
    return m_data[2];
  }
 
  inline double Quat::w() const
  {
    return m_data[3];
  }
 
  inline bool Quat::zeroRotation() const
  {
    return m_data[0] == 0.0 && m_data[1] == 0.0 && m_data[2] == 0.0 && m_data[3] == 1.0;
  }
 
 
  inline const Quat Quat::operator * (double rhs) const
  {
    return Quat(m_data[0] * rhs, m_data[1] * rhs, m_data[2] * rhs, m_data[3] * rhs);
  }
 
  inline Quat Quat::operator *= (double rhs)
  {
    m_data[0] *= rhs;
    m_data[1] *= rhs;
    m_data[2] *= rhs;
    m_data[3] *= rhs;
    return *this;        // enable nesting
  }
 
  inline const Quat Quat::operator*(const Quat& rhs) const
  {
    return Quat(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]);
  }
 
  inline Quat Quat::operator*=(const Quat& rhs)
  {
    double 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];
    double 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];
    double 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
  }
 
  inline const Quat Quat::leftMult(const Quat& rhs) const
  {
    return (*this) * rhs;
  }
 
  inline const Quat Quat::rightMult(const Quat& rhs) const
  {
    return rhs * (*this);
  }
 
 
  inline Quat Quat::operator / (double rhs) const
  {
    double div = double(1) / rhs;
    return Quat(m_data[0] * div, m_data[1] * div, m_data[2] * div, m_data[3] * div);
  }
 
  inline Quat Quat::operator /= (double rhs)
  {
    double div = double(1) / rhs;
    m_data[0] *= div;
    m_data[1] *= div;
    m_data[2] *= div;
    m_data[3] *= div;
    return *this;
  }
 
  inline const Quat Quat::operator/(const Quat& denom) const
  {
    return ((*this) * denom.inverse());
  }
 
  inline Quat Quat::operator/=(const Quat& denom)
  {
    (*this) = (*this) * denom.inverse();
    return (*this);          // enable nesting
  }
 
  inline const Quat Quat::operator + (const Quat& rhs) const
  {
    return Quat(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]);
  }
 
  inline Quat Quat::operator += (const Quat& 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
  }
 
  inline const Quat Quat::operator - (const Quat& rhs) const
  {
    return Quat(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]);
  }
 
  inline Quat Quat::operator -= (const Quat& 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
  }
 
  inline const Quat Quat::operator - () const
  {
    return Quat(-m_data[0], -m_data[1], -m_data[2], -m_data[3]);
  }
 
  inline double Quat::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]);
  }
 
  inline double Quat::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];
  }
 
  inline Quat Quat::conj() const
  {
    return Quat(-m_data[0], -m_data[1], -m_data[2], m_data[3]);
  }
 
  inline Quat Quat::inverse() const
  {
    return conj() / length2();
  }
 
  inline Quat Quat::rotate(double angle, double x, double y, double z)
  {
    return rotate(angle, Vec3(x, y, z));
  }
 
  inline Quat Quat::rotate(double angle, const Vec3& vec)
  {
    Quat q;
    q.setRotate(angle, vec);
    return q;
  }
 
  inline Quat Quat::rotate(double angle1, const Vec3& axis1,
    double angle2, const Vec3& axis2,
    double angle3, const Vec3& axis3)
  {
    Quat q;
    q.setRotate(angle1, axis1, angle2, axis2, angle3, axis3);
    return q;
  }
 
  inline Quat Quat::rotate(const Vec3& from, const Vec3& to)
  {
    Quat q;
    q.setRotate(from, to);
    return q;
  }
 
  inline Vec3 Quat::operator* (const Vec3& v) const
  {
    // nVidia SDK implementation
    Vec3 uv, uuv;
    Vec3 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;
  }
 
  inline double Quat::normalize()
  {
    double norm = this->length();
    if (norm > 0.0) {
      double inv = double(1) / norm;
      m_data[0] *= inv;
      m_data[1] *= inv;
      m_data[2] *= inv;
      m_data[3] *= inv;
    }
    return(norm);
  }
 
  template <typename T>
  inline std::ostream& operator << (std::ostream& output, const Quat& quat)
  {
    output << quat[0] << " "
      << quat[1] << " "
      << quat[2] << " "
      << quat[3];
    return output;     // to enable cascading
  }
#endif //"#ifndef SWIG"
 
  inline bool equivalent( const Quat& a, const Quat& b, double epsilon = double( MOMENTUM_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 );
  }
}
 
#ifdef _MSC_VER
# pragma warning(pop)
#endif
 
#endif /* MOMENTUM_QuatEMPLATE_H */