四元数和其余旋转方式的转换

quaternion.py

from math import *


def angleaxis_to_quat(rot, theta):
    # rot: [rx, ry, rz]
    return [r * sin(theta / 2) for r in rot] + [cos(theta / 2)]


def euler_to_quat(euler):
    # euler: [X, Y, Z]
    (X, Y, Z) = euler
    x = sin(Y/2) * sin(Z/2) * cos(X/2) + cos(Y/2) * cos(Z/2) * sin(X/2)
    y = sin(Y/2) * cos(Z/2) * cos(X/2) + cos(Y/2) * sin(Z/2) * sin(X/2)
    z = cos(Y/2) * sin(Z/2) * cos(X/2) - sin(Y/2) * cos(Z/2) * sin(X/2)
    w = cos(Y/2) * cos(Z/2) * cos(X/2) - sin(Y/2) * sin(Z/2) * sin(X/2)

    return [x, y, z, w]


def quat_to_euler(quat):
    # quat: [X, Y, Z, W]
    (X, Y, Z, W) = quat
    thres = 0.5 - 0.0009765625

    test = W * Y - X * Z
    if (test < -thres) | (test > thres):
        sign = (test > 0) - (test < 0)
        x = 0
        y = sign * pi / 2.0
        z = -2 * sign * atan2(X, W)
    else:
        x = atan2(2 * (Y * Z + W * X), W**2 - X**2 - Y**2 + Z**2)
        y = asin(2 * test)
        z = atan2(2 * (X * Y + W * Z), W**2 + X**2 - Y**2 - Z**2)

    return [x, y, z]