Quaternion.hpp

[詳解]

//
//    クオータニオン 前方定義
//
//    Copyright (c) 2022 udonrobo
//
 
 
#pragma once
 
#include <Udon/Traits/EnumerableMacro.hpp>
#include <Udon/Serializer/Serializer.hpp>
 
namespace Udon
{
 
    struct Euler;
 

    struct QuaternionDirection
    {
        bool x;
        bool y;
        bool z;
    };
 

    struct Quaternion
    {

        using ValueType = double;

        ValueType x;

        ValueType y;

        ValueType z;

        ValueType w;

        constexpr Quaternion() noexcept
            : x()
            , y()
            , z()
            , w()
        {
        }

        constexpr Quaternion(ValueType x, ValueType y, ValueType z, ValueType w) noexcept
            : x(x)
            , y(y)
            , z(z)
            , w(w)
        {
        }

        Quaternion(const Quaternion&) = default;

        Quaternion& operator=(const Quaternion&) = default;

        constexpr Quaternion operator*(const Quaternion& rhs) const noexcept
        {
            return {
                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,
                w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z,
            };
        }

        Quaternion& operator*=(const Quaternion& rhs) noexcept { return *this = *this * rhs; };

        constexpr bool operator==(const Quaternion& rhs) const noexcept
        {
            return x == rhs.x &&
                   y == rhs.y &&
                   z == rhs.z &&
                   w == rhs.w;
        };
        constexpr bool operator!=(const Quaternion& rhs) const noexcept
        {
            return !(*this == rhs);
        };

        explicit constexpr operator bool() const noexcept
        {
            return x || y || z || w;
        }

        constexpr Quaternion inverse() const noexcept
        {
            return { -x, -y, -z, w };
        }

        constexpr Quaternion directionRevision(const QuaternionDirection& direction) const noexcept
        {
            return {
                x * (direction.x ? 1 : -1),
                y * (direction.y ? 1 : -1),
                z * (direction.z ? 1 : -1),
                w
            };
        }

        static Quaternion Identity() noexcept
        {
            return { 0, 0, 0, 1 };
        }

        static Quaternion RotateX(ValueType angle) noexcept
        {
            return { sin(angle / 2), 0, 0, cos(angle / 2) };
        }

        static Quaternion RotateY(ValueType angle) noexcept
        {
            return { 0, sin(angle / 2), 0, cos(angle / 2) };
        }

        static Quaternion RotateZ(ValueType angle) noexcept
        {
            return { 0, 0, sin(angle / 2), cos(angle / 2) };
        }

        constexpr bool isZero() const noexcept
        {
            return !operator bool();
        }

        void clear() noexcept
        {
            *this = {};
        }

        Euler toEuler() const noexcept;

        double toYaw() const noexcept
        {
            return atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));
        }

        double toPitch() const noexcept
        {
            return asin(2 * (w * y - z * x));
        }

        double toRoll() const noexcept
        {
            return atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
        }
 
#ifdef SIV3D_INCLUDED

        Quaternion(const s3d::Quaternion& q) noexcept
            : x(q.getX())
            , y(q.getY())
            , z(q.getZ())
            , w(q.getW())
        {
        }

        operator s3d::Quaternion() const noexcept
        {
            return s3d::Quaternion(x, y, z, w);
        }
#endif
 
#ifdef ARDUINO
        void show() const
        {
            Serial.print(F("x: ")), Serial.print(x), Serial.print('\t');
            Serial.print(F("y: ")), Serial.print(y), Serial.print('\t');
            Serial.print(F("z: ")), Serial.print(z), Serial.print('\t');
            Serial.print(F("w: ")), Serial.print(w), Serial.print('\t');
        }
#endif
 
        UDON_ENUMERABLE(x, y, z, w)
    };
}    // namespace Udon