欧拉角和四元数相互转换

最新推荐文章于 2024-07-28 15:46:11 发布

挽手等风起

最新推荐文章于 2024-07-28 15:46:11 发布

阅读量1k

点赞数

文章标签： python 开发语言

本文链接：https://blog.csdn.net/qq_36697196/article/details/127674898

版权

import math
import numpy as np

# 欧拉角转四元数
while(1):
    which_mode = input("欧拉角转四元数输入1，四元数转欧拉角输入2。\n请输入:")
    if(which_mode == "1"):
        print("\n源坐标系到目标坐标系旋转顺序为X,Y,Z,左手系.")
        r = float(input("绕x轴旋转角度:"))
        p = float(input("绕y轴旋转角度:"))
        y = float(input("绕z轴旋转角度:"))

        sinp = math.sin(math.radians(p/2))
        siny = math.sin(math.radians(y/2))
        sinr = math.sin(math.radians(r/2))

        cosp = math.cos(math.radians(p/2))
        cosy = math.cos(math.radians(y/2))
        cosr = math.cos(math.radians(r/2))

        w = cosr*cosp*cosy + sinr*sinp*siny
        x = sinr*cosp*cosy - cosr*sinp*siny
        y = cosr*sinp*cosy + sinr*cosp*siny
        z = cosr*cosp*siny - sinr*sinp*cosy

        print("x : {}".format(x))
        print("y : {}".format(y))
        print("z : {}".format(z))