euler-angles.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2023  Christopher J. Howard
 *
 * This file is part of Antkeeper source code.
 *
 * Antkeeper source code is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Antkeeper source code is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#ifndef ANTKEEPER_MATH_EULER_ANGLES_HPP
#define ANTKEEPER_MATH_EULER_ANGLES_HPP
 
#include <engine/math/numbers.hpp>
#include <engine/math/quaternion.hpp>
#include <engine/math/vector.hpp>
#include <cmath>
#include <cstdint>
#include <concepts>
#include <utility>
 
namespace math {
 
enum class rotation_sequence: std::uint8_t
{
    zxz = 0b100010,
    
    xyx = 0b000100,
    
    yzy = 0b011001,
    
    zyz = 0b100110,
    
    xzx = 0b001000,
    
    yxy = 0b010001,
    
    xyz = 0b100100,
    
    yzx = 0b001001,
    
    zxy = 0b010010,
    
    xzy = 0b011000,
    
    zyx = 0b000110,
    
    yxz = 0b100001
};
 
template <std::integral T>
[[nodiscard]] inline constexpr vec3<T> rotation_axes(rotation_sequence sequence) noexcept
{
    const auto x = static_cast<T>(sequence);
    return {x & 3, (x >> 2) & 3, x >> 4};
}
 
template <rotation_sequence Sequence, class T>
[[nodiscard]] vec3<T> euler_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    constexpr auto axes = rotation_axes<int>(Sequence);
    constexpr auto proper = axes[0] == axes[2];
    constexpr auto i = axes[0];
    constexpr auto j = axes[1];
    constexpr auto k = proper ? 3 - i - j : axes[2];
    constexpr auto sign = static_cast<T>(((i - j) * (j - k) * (k - i)) >> 1);
    
    auto a = q.r;
    auto b = q.i[i];
    auto c = q.i[j];
    auto d = q.i[k] * sign;
    if constexpr (!proper)
    {
        a -= q.i[j];
        b += d;
        c += q.r;
        d -= q.i[i];
    }
    const auto aa_bb = a * a + b * b;
    
    vec3<T> angles;
    angles[1] = std::acos(T{2} * aa_bb / (aa_bb + c * c + d * d) - T{1});
    
    if (std::abs(angles[1]) <= tolerance)
    {
        angles[0] = T{0};
        angles[2] = std::atan2(b, a) * T{2};
    }
    else if (std::abs(angles[1] - pi<T>) <= tolerance)
    {
        angles[0] = T{0};
        angles[2] = std::atan2(-d, c) * T{-2};
    }
    else
    {
        const auto half_sum = std::atan2(b, a);
        const auto half_diff = std::atan2(-d, c);
        
        angles[0] = half_sum + half_diff;
        angles[2] = half_sum - half_diff;
    }
    
    if constexpr (!proper)
    {
        angles[2] *= sign;
        angles[1] -= half_pi<T>;
    }
    
    return angles;
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_zxz_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::zxz, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_xyx_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::xyx, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_yzy_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::yzy, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_zyz_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::zyz, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_xzx_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::xzx, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_yxy_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::yxy, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_xyz_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::xyz, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_yzx_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::yzx, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_zxy_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::zxy, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_xzy_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::xzy, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_zyx_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::zyx, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_yxz_from_quat(const quat<T>& q, T tolerance = T{1e-6})
{
    return euler_from_quat<rotation_sequence::yxz, T>(q, tolerance);
}
 
template <class T>
[[nodiscard]] inline vec3<T> euler_from_quat(rotation_sequence sequence, const quat<T>& q, T tolerance = T{1e-6})
{
    switch (sequence)
    {
        case rotation_sequence::zxz:
            return euler_zxz_from_quat<T>(q, tolerance);
        case rotation_sequence::xyx:
            return euler_xyx_from_quat<T>(q, tolerance);
        case rotation_sequence::yzy:
            return euler_yzy_from_quat<T>(q, tolerance);
        case rotation_sequence::zyz:
            return euler_zyz_from_quat<T>(q, tolerance);
        case rotation_sequence::xzx:
            return euler_xzx_from_quat<T>(q, tolerance);
        case rotation_sequence::yxy:
            return euler_yxy_from_quat<T>(q, tolerance);
        case rotation_sequence::xyz:
            return euler_xyz_from_quat<T>(q, tolerance);
        case rotation_sequence::yzx:
            return euler_yzx_from_quat<T>(q, tolerance);
        case rotation_sequence::zxy:
            return euler_zxy_from_quat<T>(q, tolerance);
        case rotation_sequence::xzy:
            return euler_xzy_from_quat<T>(q, tolerance);
        case rotation_sequence::zyx:
            return euler_zyx_from_quat<T>(q, tolerance);
        case rotation_sequence::yxz:
        default:
            return euler_yxz_from_quat<T>(q, tolerance);
    }
}
 
 
template <rotation_sequence Sequence, class T>
[[nodiscard]] quat<T> euler_to_quat(const vec3<T>& angles)
{
    const auto half_angles = angles * T{0.5};
    const T cx = std::cos(half_angles.x());
    const T sx = std::sin(half_angles.x());
    const T cy = std::cos(half_angles.y());
    const T sy = std::sin(half_angles.y());
    const T cz = std::cos(half_angles.z());
    const T sz = std::sin(half_angles.z());
    
    if constexpr (Sequence == rotation_sequence::zxz)
    {
        return
        {
            cx * cy * cz - sx * cy * sz,
            cx * cz * sy + sx * sy * sz,
            cx * sy * sz - sx * cz * sy,
            cx * cy * sz + cy * cz * sx
        };
    }
    else if constexpr (Sequence == rotation_sequence::xyx)
    {
        return
        {
            cx * cy * cz - sx * cy * sz,
            cx * cy * sz + cy * cz * sx,
            cx * cz * sy + sx * sy * sz,
            cx * sy * sz - sx * cz * sy
        };
    }
    else if constexpr (Sequence == rotation_sequence::yzy)
    {
        return
        {
            cx * cy * cz - sx * cy * sz,
            cx * sy * sz - sx * cz * sy,
            cx * cy * sz + cy * cz * sx,
            cx * cz * sy + sx * sy * sz
        };
    }
    else if constexpr (Sequence == rotation_sequence::zyz)
    {
        return
        {
            cx * cy * cz - sx * cy * sz,
            -cx * sy * sz + sx * cz * sy,
            cx * cz * sy + sx * sy * sz,
            cx * cy * sz + cy * cz * sx
        };
    }
    else if constexpr (Sequence == rotation_sequence::xzx)
    {
        return
        {
            cx * cy * cz - sx * cy * sz,
            cx * cy * sz + cy * cz * sx,
            -cx * sy * sz + sx * cz * sy,
            cx * cz * sy + sx * sy * sz
        };
    }
    else if constexpr (Sequence == rotation_sequence::yxy)
    {
        return
        {
            cx * cy * cz - sx * cy * sz,
            cx * cz * sy + sx * sy * sz,
            cx * cy * sz + cy * cz * sx,
            -cx * sy * sz + sx * cz * sy
        };
    }
    else if constexpr (Sequence == rotation_sequence::xyz)
    {
        return
        {
            cx * cy * cz + sx * sy * sz,
            -cx * sy * sz + cy * cz * sx,
            cx * cz * sy + sx * cy * sz,
            cx * cy * sz - sx * cz * sy
        };
    }
    else if constexpr (Sequence == rotation_sequence::yzx)
    {
        return
        {
            cx * cy * cz + sx * sy * sz,
            cx * cy * sz - sx * cz * sy,
            -cx * sy * sz + cy * cz * sx,
            cx * cz * sy + sx * cy * sz
        };
    }
    else if constexpr (Sequence == rotation_sequence::zxy)
    {
        return
        {
            cx * cy * cz + sx * sy * sz,
            cx * cz * sy + sx * cy * sz,
            cx * cy * sz - sx * cz * sy,
            -cx * sy * sz + cy * cz * sx
        };
    }
    else if constexpr (Sequence == rotation_sequence::xzy)
    {
        return
        {
            cx * cy * cz - sx * sy * sz,
            cx * sy * sz + cy * cz * sx,
            cx * cy * sz + sx * cz * sy,
            cx * cz * sy - sx * cy * sz
        };
    }
    else if constexpr (Sequence == rotation_sequence::zyx)
    {
        return
        {
            cx * cy * cz - sx * sy * sz,
            cx * cy * sz + sx * cz * sy,
            cx * cz * sy - sx * cy * sz,
            cx * sy * sz + cy * cz * sx
        };
    }
    else //if constexpr (Sequence == rotation_sequence::yxz)
    {
        return
        {
            cx * cy * cz - sx * sy * sz,
            cx * cz * sy - sx * cy * sz,
            cx * sy * sz + cy * cz * sx,
            cx * cy * sz + sx * cz * sy
        };
    }
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_zxz_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::zxz, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_xyx_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::xyx, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_yzy_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::yzy, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_zyz_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::zyz, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_xzx_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::xzx, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_yxy_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::yxy, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_xyz_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::xyz, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_yzx_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::yzx, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_zxy_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::zxy, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_xzy_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::xzy, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_zyx_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::zyx, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_yxz_to_quat(const vec3<T>& angles)
{
    return euler_to_quat<rotation_sequence::yxz, T>(angles);
}
 
template <class T>
[[nodiscard]] inline quat<T> euler_to_quat(rotation_sequence sequence, const vec3<T>& angles)
{
    switch (sequence)
    {
        case rotation_sequence::zxz:
            return euler_zxz_to_quat<T>(angles);
        case rotation_sequence::xyx:
            return euler_xyx_to_quat<T>(angles);
        case rotation_sequence::yzy:
            return euler_yzy_to_quat<T>(angles);
        case rotation_sequence::zyz:
            return euler_zyz_to_quat<T>(angles);
        case rotation_sequence::xzx:
            return euler_xzx_to_quat<T>(angles);
        case rotation_sequence::yxy:
            return euler_yxy_to_quat<T>(angles);
        case rotation_sequence::xyz:
            return euler_xyz_to_quat<T>(angles);
        case rotation_sequence::yzx:
            return euler_yzx_to_quat<T>(angles);
        case rotation_sequence::zxy:
            return euler_zxy_to_quat<T>(angles);
        case rotation_sequence::xzy:
            return euler_xzy_to_quat<T>(angles);
        case rotation_sequence::zyx:
            return euler_zyx_to_quat<T>(angles);
        case rotation_sequence::yxz:
        default:
            return euler_yxz_to_quat<T>(angles);
    }
}
 
} // namespace math
 
#endif // ANTKEEPER_MATH_EULER_ANGLES_HPP