xsquaternion.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#ifndef XSQUATERNION_H
#define XSQUATERNION_H

#include "xsmath.h"

struct XsEuler;
struct XsMatrix;
struct XsVector;
struct XsQuaternion;

#ifdef __cplusplus
extern "C"
{
#else
#define XSQUATERNION_INITIALIZER                                   \
    {                                                              \
        {                                                          \
            {                                                      \
                XsMath_zero, XsMath_zero, XsMath_zero, XsMath_zero \
            }                                                      \
        }                                                          \
    }
typedef struct XsQuaternion XsQuaternion;
#endif

    XSTYPES_DLL_API void XsQuaternion_destruct(XsQuaternion* thisPtr);
    XSTYPES_DLL_API int XsQuaternion_empty(const XsQuaternion* thisPtr);
    XSTYPES_DLL_API void XsQuaternion_inverse(
        const XsQuaternion* thisPtr, XsQuaternion* dest);
    XSTYPES_DLL_API XsReal XsQuaternion_normalized(
        const XsQuaternion* thisPtr, XsQuaternion* dest);
    XSTYPES_DLL_API XsReal XsQuaternion_normalize(XsQuaternion* thisPtr);
    XSTYPES_DLL_API void XsQuaternion_fromEulerAngles(
        XsQuaternion* thisPtr, const struct XsEuler* src);
    XSTYPES_DLL_API void XsQuaternion_fromRotationMatrix(
        XsQuaternion* thisPtr, const struct XsMatrix* ori);
    XSTYPES_DLL_API const XsQuaternion* XsQuaternion_identity(void);
    XSTYPES_DLL_API void XsQuaternion_multiply(
        const XsQuaternion* left, const XsQuaternion* right,
        XsQuaternion* dest);
    XSTYPES_DLL_API void XsQuaternion_swap(XsQuaternion* a, XsQuaternion* b);
    XSTYPES_DLL_API void XsQuaternion_copy(
        XsQuaternion* copy, XsQuaternion const* src);
    XSTYPES_DLL_API int XsQuaternion_equal(
        XsQuaternion const* a, XsQuaternion const* b);

#ifdef __cplusplus
}  // extern "C"
#endif

struct XsQuaternion
{
    XSCPPPROTECTED
    union {
        struct
        {
            /** Stores the w component of the quaternion */
            XsReal m_w;
            /** Stores the x component of the quaternion */
            XsReal m_x;
            /** Stores the y component of the quaternion */
            XsReal m_y;
            /** Stores the z component of the quaternion */
            XsReal m_z;
        };
        /** Stores the quaternion in an array of four elements */
        XsReal m_data[4];
    };
#ifdef __cplusplus
   public:
    //! \brief Construct a quaternion with the supplied values, or zero's by
    //! default
    inline explicit XsQuaternion(
        XsReal w = XsMath_zero, XsReal x = XsMath_zero, XsReal y = XsMath_zero,
        XsReal z = XsMath_zero)
        : m_w(w), m_x(x), m_y(y), m_z(z)
    {
    }

    //! \brief Construct a quaternion with the supplied values from the \a other
    //! Quaternion
    inline XsQuaternion(const XsQuaternion& other)
        : m_w(other.m_w), m_x(other.m_x), m_y(other.m_y), m_z(other.m_z)
    {
    }

    //! \brief Construct a quaternion by converting from an XsEuler object
    inline explicit XsQuaternion(const XsEuler& euler)
    {
        XsQuaternion_fromEulerAngles(this, &euler);
    }

    //! \brief Construct a quaternion by converting from an XsMatrix rotation
    //! matrix object
    inline explicit XsQuaternion(const XsMatrix& ori)
    {
        XsQuaternion_fromRotationMatrix(this, &ori);
    }

    //! \brief Assigns the \a other quaternion to this quaternion
    inline XsQuaternion& operator=(const XsQuaternion& other)
    {
        m_w = other.m_w;
        m_x = other.m_x;
        m_y = other.m_y;
        m_z = other.m_z;
        return *this;
    }

    /*! \brief Set the Quaternion to these specific values
     */
    inline void assign(XsReal w, XsReal x, XsReal y, XsReal z)
    {
        m_w = w;
        m_x = x;
        m_y = y;
        m_z = z;
    }

    /*! \brief Set the Quaternion to the specific values in the supplied array.
        \param values An array that contains at least 4 items. The first four
       will be interpreted as w,x,y,z
    */
    inline void assign(const XsReal* values)
    {
        for (int i = 0; i < 4; ++i) m_data[i] = values[i];
    }

    //! \brief Returns a reference to the \a index'th component of the
    //! quaternion
    inline XsReal& operator[](XsSize index)
    {
        assert(index < 4);
        return m_data[index];
    }

    //! \brief Returns the \a index'th component of the quaternion
    inline XsReal const& operator[](XsSize index) const
    {
        assert(index < 4);
        return m_data[index];
    }

    //! \brief Return a const pointer to the internal data
    inline const XsReal* data() const { return m_data; }
    //! \brief \copybrief XsQuaternion_inverse \returns The inverse/conjugate of
    //! the quaternion
    inline XsQuaternion inverse() const
    {
        XsQuaternion tmp;
        XsQuaternion_inverse(this, &tmp);
        return tmp;
    }

    //! \brief \copybrief XsQuaternion_inverse \returns The inverse/conjugate of
    //! the quaternion
    inline XsQuaternion conjugate() const
    {
        XsQuaternion tmp;
        XsQuaternion_inverse(this, &tmp);
        return tmp;
    }

    //! \brief Return a normalized version of the quaternion \sa
    //! XsQuaternion_normalized \returns The normalized quaternion
    XsQuaternion normalized() const
    {
        XsQuaternion tmp;
        XsQuaternion_normalized(this, &tmp);
        return tmp;
    }

    //! \brief Normalized the quaternion \sa XsQuaternion_normalized \returns
    //! The cartesian length of the quaternion before normalization
    inline XsReal normalize() { return XsQuaternion_normalized(this, this); }
    //! \brief \copybrief XsQuaternion_empty
    inline bool empty() const { return 0 != XsQuaternion_empty(this); }
    //! \brief \copybrief XsQuaternion_fromEulerAngles
    inline XsQuaternion& fromEulerAngles(const XsEuler& src)
    {
        XsQuaternion_fromEulerAngles(this, &src);
        return *this;
    }

    //! \brief \copybrief XsQuaternion_fromRotationMatrix
    inline XsQuaternion& fromRotationMatrix(const XsMatrix& ori)
    {
        XsQuaternion_fromRotationMatrix(this, &ori);
        return *this;
    }

    //! \brief \copybrief XsQuaternion_identity
    inline static const XsQuaternion& identity()
    {
        return *XsQuaternion_identity();
    }

    /*! \brief In-place multiplication of this quaternion with \a other
       quaternion
        \param other The other quaternion to multiply with (right side of
       multiplication)
        \sa XsQuaternion_multiply()
    */
    inline void operator*=(const XsQuaternion& other)
    {
        XsQuaternion_multiply(this, &other, this);
    }

    //! \brief Return the w component of the quaternion
    inline XsReal w() const { return m_w; }
    //! \brief Return the x component of the quaternion
    inline XsReal x() const { return m_x; }
    //! \brief Return the y component of the quaternion
    inline XsReal y() const { return m_y; }
    //! \brief Return the z component of the quaternion
    inline XsReal z() const { return m_z; }
    //! \brief Return a reference to the w component of the quaternion
    inline XsReal& w() { return m_w; }
    //! \brief Return a reference to the x component of the quaternion
    inline XsReal& x() { return m_x; }
    //! \brief Return a reference to the y component of the quaternion
    inline XsReal& y() { return m_y; }
    //! \brief Return a reference to the z component of the quaternion
    inline XsReal& z() { return m_z; }
    //! \brief Swap the contents with \a other
    inline void swap(XsQuaternion& other) { XsQuaternion_swap(this, &other); }
    //! \brief Returns true if \a other is numerically identical to this
    inline bool operator==(const XsQuaternion& other) const
    {
        return m_w == other.m_w && m_x == other.m_x && m_y == other.m_y &&
               m_z == other.m_z;
    }

#endif
};

#ifdef __cplusplus
//! \brief Return the negated version of the Quaternion \a q (w,-x,-y,-z)
inline XsQuaternion operator-(const XsQuaternion& q)
{
    return XsQuaternion(q.w(), -q.x(), -q.y(), -q.z());
}

//! \brief Multiply \a lhs by \a rhs and return the result
inline XsQuaternion operator*(const XsQuaternion& lhs, const XsQuaternion& rhs)
{
    XsQuaternion tmp(lhs);
    tmp *= rhs;
    return tmp;
}
#endif

#endif  // file guard