Quaternion from ZYX intrisic Euler angles (Z rot, then local -Y rot, then local X rot)

zyx-quaternion.py

import numpy


def zyx_to_quaternion(z_angle, y_angle, x_angle):
    cos_x, sin_x = numpy.cos(x_angle / 2), numpy.sin(x_angle / 2)
    cos_y, sin_y = numpy.cos(y_angle / 2), numpy.sin(y_angle / 2)
    cos_z, sin_z = numpy.cos(z_angle / 2), numpy.sin(z_angle / 2)
    
    return numpy.array([
        cos_x * cos_y * cos_z - sin_x * sin_y * sin_z,
        sin_x * cos_y * cos_z + cos_x * sin_y * sin_z,
        sin_x * cos_y * sin_z - cos_x * sin_y * cos_z,
        sin_x * sin_y * cos_z + cos_x * cos_y * sin_z,        
    ])
  
  
  def quaternion_to_zyx(Q):
    a, b, c, d = Q

    y_angle = numpy.arcsin(numpy.clip(2 * (b * d - c * a), -1., 1.))
    x_angle = numpy.arctan2(2 * (c * d + a * b), 1. - 2 * (b ** 2 + c ** 2))
    z_angle = numpy.arctan2(2 * (b * c + d * a), 1. - 2 * (c ** 2 + d ** 2))
    
    return z_angle, y_angle, x_angle