Untitled

import numpy as np

def rotation_matrix_to_euler_angles(rotation_matrix):
    """
    Преобразует матрицу поворота в Эйлеровы углы (порядок Y-X-Z) с использованием кватернионов.
    """

    # Преобразование матрицы в кватернион (одной из стандартных формул)
    trace = np.trace(rotation_matrix)
    if trace > 0:
        s = 0.5 / np.sqrt(trace + 1.0)
        w = 0.25 / s
        x = (rotation_matrix[2, 1] - rotation_matrix[1, 2]) * s
        y = (rotation_matrix[0, 2] - rotation_matrix[2, 0]) * s
        z = (rotation_matrix[1, 0] - rotation_matrix[0, 1]) * s
    else:
        if rotation_matrix[0, 0] > rotation_matrix[1, 1] and rotation_matrix[0, 0] > rotation_matrix[2, 2]:
            s = 2.0 * np.sqrt(1.0 + rotation_matrix[0, 0] - rotation_matrix[1, 1] - rotation_matrix[2, 2])
            w = (rotation_matrix[2, 1] - rotation_matrix[1, 2]) / s
            x = 0.25 * s
            y = (rotation_matrix[0, 1] + rotation_matrix[1, 0]) / s
            z = (rotation_matrix[0, 2] + rotation_matrix[2, 0]) / s
        elif rotation_matrix[1, 1] > rotation_matrix[2, 2]:
            s = 2.0 * np.sqrt(1.0 + rotation_matrix[1, 1] - rotation_matrix[0, 0] - rotation_matrix[2, 2])
            w = (rotation_matrix[0, 2] - rotation_matrix[2, 0]) / s
            x = (rotation_matrix[0, 1] + rotation_matrix[1, 0]) / s
            y = 0.25 * s
            z = (rotation_matrix[1, 2] + rotation_matrix[2, 1]) / s
        else:
            s = 2.0 * np.sqrt(1.0 + rotation_matrix[2, 2] - rotation_matrix[0, 0] - rotation_matrix[1, 1])
            w = (rotation_matrix[1, 0] - rotation_matrix[0, 1]) / s
            x = (rotation_matrix[0, 2] + rotation_matrix[2, 0]) / s
            y = (rotation_matrix[1, 2] + rotation_matrix[2, 1]) / s
            z = 0.25 * s

    # Нормализация кватерниона
    norm = np.sqrt(w**2 + x**2 + y**2 + z**2)
    w /= norm
    x /= norm
    y /= norm
    z /= norm

    # Преобразование кватерниона в Эйлеровы углы (порядок Y-X-Z)
    alpha = np.arctan2(2.0 * (w*y + x*z), 1.0 - 2.0 * (y*y + z*z))  # Угол вокруг Y
    beta  = np.arcsin(2.0 * (w*x - y*z))  # Угол вокруг X
    gamma = np.arctan2(2.0 * (w*z + x*y), 1.0 - 2.0 * (x*x + z*z))  # Угол вокруг Z

    return alpha, beta, gamma


# Пример использования:
rotation_matrix = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])  # Пример матрицы поворота

alpha, beta, gamma = rotation_matrix_to_euler_angles(rotation_matrix)
print(f"Углы Эйлера (Y-X-Z): α={alpha}, β={beta}, γ={gamma}")