欧拉角和四元数相互转换

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*sinyx = sinr*cosp*cosy - cosr*sinp*sinyy = cosr*sinp*cosy + sinr*cosp*sinyz = cosr*cosp*siny - sinr*sinp*cosyprint("x : {}".format(x))print("y : {}".format(y))print("z : {}".format(z))print("w : {}".format(w))elif(which_mode == "2"):print("请按顺序输入4元数")x = input("x:")y = input("y:")z = input("z:")w = input("w:")x = float(x)y = float(y)z = float(z)w = float(w)r = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))r = r / math.pi * 180p = math.asin(2 * (w * y - z * x))p = p / math.pi * 180y = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))y = y / math.pi * 180print("\n源坐标系到目标坐标系旋转顺序为X,Y,Z,左手系．")print("绕x轴旋转角度: {}".format(r))print("绕y轴旋转角度: {}".format(p))print("绕z轴旋转角度: {}\n".format(y))from math import cos, sin, pi, atan2 ,asin
#欧拉角(x, y, z)转换为四元数(q0, q1, q2, q3)
#x , y , z 单位为角度
x, y, z  = 90 ,0 ,90
x, y, z  = x*pi/180 ,y*pi/180 ,z*pi/180
q0 ,q1 ,q2 ,q3 = 0 ,0 ,0 ,0
q0 = cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2)
q1 = sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2)
q2 = cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2)
q3 = cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)
print('欧拉角({0:f}, {1:f}, {2:f})转换为四元数(q0, q1, q2, q3)'.format(x*180/pi, y*180/pi, z*180/pi))
print("q0 = {0:f}".format(q0))
print("q1 = {0:f}".format(q1))
print("q2 = {0:f}".format(q2))
print("q3 = {0:f}".format(q3))
print() #欧拉角(x, y, z)转换为旋转矩阵
x, y, z  = 90 ,0 ,90
x, y, z  = x*pi/180 ,y*pi/180 ,z*pi/180
r11 ,r12 ,r13 = cos(z)*cos(y), cos(z)*sin(y)*sin(x) - sin(z)*cos(x), cos(z)*sin(y)*cos(x) + sin(z)*sin(x)
r21 ,r22 ,r23 = sin(z)*cos(y), sin(z)*sin(y)*sin(x) + cos(z)*cos(x), sin(z)*sin(y)*cos(x) - cos(z)*sin(x)
r31 ,r32 ,r33 = -sin(y), cos(y)*sin(x), cos(y)*cos(x)
print('欧拉角({0:f}, {1:f}, {2:f})转换为旋转矩阵'.format(x*180/pi, y*180/pi, z*180/pi))
print('{0:f} , {1:f} , {2:f}'.format(r11 ,r12 ,r13))
print('{0:f} , {1:f} , {2:f}'.format(r21 ,r22 ,r23))
print('{0:f} , {1:f} , {2:f}'.format(r31 ,r32 ,r33))
print()

参考：
Python - 欧拉角 - 四元素 - 旋转矩阵
python 四元数转欧拉角

欧拉角和四元数相互转换相关推荐

四元数，欧拉角，旋转矩阵相互转换
#include <TransForms3d/TransForms.h>/*---------------------------------------角度弧度转换----------- ...
3D数学基础——矩阵、欧拉角和四元数的相互转换与比较
矩阵.欧拉角和四元数的相互转换与比较相互转换这里只展示最终的转换结果,推导过程请参考<3D数学基础:图形与游戏开发> 欧拉角转换到矩阵欧拉角描述了一个旋转序列,分别计算出给每个旋转的 ...
欧拉角与四元数互转，及四元数slerp球面线性插值算法
欧拉角与四元数互转,及四元数slerp球面线性插值算法 1. 欧拉角与四元数是什么? 2. 源码 2.1 欧拉角类 2.2 四元数类 2.3 欧拉角与四元数互转及球面线性插值算法参考 1. 欧拉角与 ...
旋转矩阵、欧拉角、四元数比较
旋转矩阵.欧拉角.四元数主要用于:向量的旋转.坐标系之间的转换.角位移计算.方位的平滑插值计算. 不同的方位表示方法适用于不同的情况.下面是我们对合理选择格式的一些建议: l 欧拉角最容易使用.当需 ...
深入浅出无人机姿态，欧拉角，四元数，指数表示及数据转换与程序实现
很多朋友留言或私信问到书名和出版日期.先感谢这么多朋友的支持和信任,MR.城堡会努力为大家带来更多干货.另外,交稿日期是今年12月,出版要看机械工业出版社的安排,书名和出版情况确定后,会在专栏告知大家 ...
旋转矩阵、欧拉角、四元数理论及其转换关系
旋转矩阵.欧拉角.四元数理论及其转换关系 author@jason_ql(lql0716) http://blog.csdn.net/lql0716 1.概述旋转矩阵.欧拉角.四元数主要用于表示坐标 ...
3D 中的方位与角位移(旋转矩阵、欧拉角、四元数)
文章目录一.3D 中的方位与角位移 1. 欧拉角 (Euler angles) 2. 四元数的相关知识 2.1 复数 2.2 欧拉旋转定理 2.3 三维空间旋转的拆分 3. 四元数 (Quatern ...
欧拉角和四元数之间是如何转换的？
规定: 1.航空次序欧拉角: Z轴(航偏角,yaw,Ψ) Y轴(俯仰角,pitch,θ) X轴(滚转角,roll,Φ) 2.导航坐标系(N系)为O-ENU坐标系,即东北天坐标系.且机体坐标系初始 ...
欧拉角和四元数之间转换公式推导
前言首先明确一点不同的旋转顺序,欧拉角和四元数之间转换的公式是不一样的,以下推导使用的旋转顺序为:,,. 1.欧拉角转四元数绕X轴旋转角度为,绕Y轴旋转角度为,绕Z轴旋转角度为,把这三个角轴转换为 ...

欧拉角和四元数相互转换

欧拉角和四元数相互转换相关推荐

最新文章

热门文章