quaternion.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
 *
 * SPDX-License-Identifier: MIT
 */
#ifndef M5_UNIT_IMU_MATH_QUATERNION_HPP
#define M5_UNIT_IMU_MATH_QUATERNION_HPP
 
#include <cmath>
 
#include "vector3.hpp"
 
namespace m5 {
namespace math {
 
constexpr float QUATERNION_ZERO_EPSILON{1e-9f};
constexpr float QUATERNION_PI{3.14159265358979323846f};
 
struct Quaternion {
    float w{1.0f};  
    float x{};      
    float y{};      
    float z{};      
 
    Quaternion() = default;
    constexpr Quaternion(const float w_, const float x_, const float y_, const float z_) : w(w_), x(x_), y(y_), z(z_)
    {
    }
 
    static inline Quaternion identity()
    {
        return Quaternion();
    }
 
    inline Quaternion operator*(const Quaternion& rhs) const
    {
        return Quaternion(w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z, w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y,
                          w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x, w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w);
    }
 
    inline Quaternion conjugated() const
    {
        return Quaternion(w, -x, -y, -z);
    }
 
    inline float normSq() const
    {
        return w * w + x * x + y * y + z * z;
    }
    inline float norm() const
    {
        return std::sqrt(normSq());
    }
    bool normalize()
    {
        const float n = norm();
        if (n < QUATERNION_ZERO_EPSILON) {
            return false;
        }
        const float inv = 1.0f / n;
        w *= inv;
        x *= inv;
        y *= inv;
        z *= inv;
        return true;
    }
    inline Quaternion normalized() const
    {
        Quaternion q(*this);
        q.normalize();
        return q;
    }
 
    inline Vector3 rotate(const Vector3& v) const
    {
        const Vector3 qv(x, y, z);
        const Vector3 t = qv.cross(v) * 2.0f;
        return v + t * w + qv.cross(t);
    }
 
    static inline Quaternion fromAxisAngle(const Vector3& axis, const float angle_rad)
    {
        if (axis.norm() < QUATERNION_ZERO_EPSILON) {
            return identity();
        }
        const Vector3 a  = axis.normalized();
        const float half = angle_rad * 0.5f;
        const float s    = std::sin(half);
        return Quaternion(std::cos(half), a.x * s, a.y * s, a.z * s);
    }
 
    static inline Quaternion fromEuler(const float roll_deg, const float pitch_deg, const float yaw_deg)
    {
        constexpr float deg2rad{QUATERNION_PI / 180.0f};
        const float roll  = roll_deg * deg2rad;
        const float pitch = pitch_deg * deg2rad;
        const float yaw   = yaw_deg * deg2rad;
 
        const float cr = std::cos(roll * 0.5f);
        const float sr = std::sin(roll * 0.5f);
        const float cp = std::cos(pitch * 0.5f);
        const float sp = std::sin(pitch * 0.5f);
        const float cy = std::cos(yaw * 0.5f);
        const float sy = std::sin(yaw * 0.5f);
 
        return Quaternion(cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, cr * sp * cy + sr * cp * sy,
                          cr * cp * sy - sr * sp * cy);
    }
 
    inline void toEuler(float& roll_deg, float& pitch_deg, float& yaw_deg) const
    {
        constexpr float rad2deg{180.0f / QUATERNION_PI};
 
        const float sinr_cosp = 2.0f * (w * x + y * z);
        const float cosr_cosp = 1.0f - 2.0f * (x * x + y * y);
        const float roll      = std::atan2(sinr_cosp, cosr_cosp);
 
        float sinp = 2.0f * (w * y - z * x);
        if (sinp > 1.0f) {
            sinp = 1.0f;
        } else if (sinp < -1.0f) {
            sinp = -1.0f;
        }
        const float pitch = std::asin(sinp);
 
        const float siny_cosp = 2.0f * (w * z + x * y);
        const float cosy_cosp = 1.0f - 2.0f * (y * y + z * z);
        const float yaw       = std::atan2(siny_cosp, cosy_cosp);
 
        roll_deg  = roll * rad2deg;
        pitch_deg = pitch * rad2deg;
        yaw_deg   = yaw * rad2deg;
    }
 
    inline void integrate(const Vector3& gyro_rad_s, const float dt)
    {
        const Quaternion omega(0.0f, gyro_rad_s.x, gyro_rad_s.y, gyro_rad_s.z);
        const Quaternion qdot = (*this) * omega;
        const float half_dt   = 0.5f * dt;
        w += half_dt * qdot.w;
        x += half_dt * qdot.x;
        y += half_dt * qdot.y;
        z += half_dt * qdot.z;
        normalize();
    }
};
 
}  // namespace math
}  // namespace m5
#endif