math_quaternion.h

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#ifndef DESIGNLAB_MATH_QUATERNION_H_
#define DESIGNLAB_MATH_QUATERNION_H_
 
#include <cmath>
#include <string>
 
#include "cassert_define.h"
#include "math_euler.h"
#include "math_util.h"
#include "math_vector3.h"
 
 
namespace designlab
{
 
struct Quaternion final
{
    constexpr Quaternion() : w(1.0f), v(0.0f, 0.0f, 0.0f) {}
 
    constexpr Quaternion(const float w_,
        const float x_, const float y_, const float z_) :
        w(w_),
        v(x_, y_, z_) {}
 
    constexpr Quaternion(const float w_, const Vector3& v_) : w(w_), v(v_) {}
 
    constexpr Quaternion(const Quaternion& q) = default;
 
    constexpr Quaternion(Quaternion&& q) noexcept = default;
 
    constexpr Quaternion& operator =(const Quaternion& q) = default;
 
    constexpr Quaternion operator + () const noexcept { return *this; }
    constexpr Quaternion operator - () const noexcept { return { -w, -v }; }
    constexpr Quaternion operator + (const Quaternion& q) const noexcept
    {
        return { w + q.w, v + q.v };
    }
    constexpr Quaternion operator - (const Quaternion& q) const noexcept
    {
        return { w - q.w, v - q.v };
    }
    constexpr Quaternion operator * (const Quaternion& q) const noexcept
    {
        return { w * q.w - v.Dot(q.v), w * q.v + q.w * v + v.Cross(q.v) };
    }
    constexpr Quaternion operator * (const float s) const noexcept
    {
        return { w * s, v * s };
    }
    constexpr Quaternion operator / (const float s) const
    {
        return { w / s, v / s };
    }
 
    bool operator == (const Quaternion& q) const noexcept
    {
        return math_util::IsEqual(w, q.w) && (v == q.v);
    }
    bool operator != (const Quaternion& q) const noexcept { return !(*this == q); }
 
    bool operator < (const Quaternion& q) const noexcept
    {
        return (w < q.w) || (w == q.w && v < q.v);
    }
    bool operator > (const Quaternion& other) const noexcept
    {
        return other < *this;
    }
 
    bool operator <= (const Quaternion& other) const noexcept
    {
        return !(*this > other);
    }
 
    bool operator >= (const Quaternion& other) const noexcept
    {
        return !(*this < other);
    }
 
 
    [[nodiscard]]
    constexpr float Dot(Quaternion other) const noexcept
    {
        return w * other.w + v.Dot(other.v);
    }
 
    [[nodiscard]]
    constexpr Quaternion GetConjugate() const noexcept { return { w, -v }; }
 
    [[nodiscard]]
    constexpr float GetLengthSquared() const noexcept
    {
        return w * w + v.Dot(v);
    }
 
    [[nodiscard]]
    inline float GetNorm() const noexcept
    {
        return std::sqrt(GetLengthSquared());
    }
 
    [[nodiscard]]
    inline Quaternion GetInverse() const
    {
        return GetConjugate() * (1 / GetNorm());
    }
 
    [[nodiscard]] Quaternion GetNormalized() const noexcept;
 
    [[nodiscard]]
    constexpr float GetDistanceSquared(const Quaternion& q) const noexcept
    {
        return (*this - q).GetLengthSquared();
    }
 
    inline void Normalize() noexcept { *this = GetNormalized(); }
 
    [[nodiscard]] constexpr Quaternion ToLeftHandCoordinate() const noexcept
    {
        return { w, v.x, -v.y, v.z };
    }
 
    [[nodiscard]]
    static Quaternion MakeByAngleAxis(float rad_angle, const Vector3& axis);
 
 
    [[nodiscard]] std::string ToString() const;
 
    [[nodiscard]] std::string ToCsvString() const;
 
    float w;    
    Vector3 v;  
};
 
 
constexpr Quaternion operator * (float s, const Quaternion& q) { return q * s; }
 
template <class Char>
inline std::basic_ostream<Char>& operator <<(
    std::basic_ostream<Char>& os, const Quaternion& q)
{
    os << math_util::FloatingPointNumToString(q.w) << Char(',') << q.v;
 
    return os;
}
 
template <class Char>
inline std::basic_istream<Char>& operator >>(
    std::basic_istream<Char>& is, Quaternion& q)
{
    Char unused = 0;
    return is >> unused >> q.w >> unused >> q.v;
}
 
[[nodiscard]]
constexpr Vector3 RotateVector3(const Vector3& vec, const Quaternion& q)
{
    // 回転させるベクトルをスカラーが0のクオータニオンに変換．
    const Quaternion p{ 0, vec.x, vec.y, vec.z };
 
    // 正規化されたクォータニオンを使う場合は，共役と逆数が等しいので，
    // 計算量を減らすことができる．
 
    // 正規化されたクォータニオンを使う場合は，正規化されたクォータニオンを渡す必要がある．
    assert(math_util::IsEqual(q.GetNorm(), 1.0f));
 
    return (q * p * q.GetConjugate()).v;  // ベクトル成分を返す．
}
 
Quaternion SlerpQuaternion(const Quaternion& q1, const Quaternion& q2, float t);
 
}  // namespace designlab
 
 
#endif  // DESIGNLAB_MATH_QUATERNION_H_