2021-02-06

空间向量计算欧拉角

已知空间内两个向量，求从一个向量变换到另一个向量的空间坐标变换矩阵或欧拉角
V目标=martix[ 3*3矩阵 ]*V当前

```python
import numpy as np
import math
#注通过空间向量求欧拉角，与通过空间向量坐标转换求欧拉角结果一样，少了一个自由度
# p1为向量起点，p2为向量终点
p1 = np.array([[9.8113], [4.85333], [3.58656]])
p2 = np.array([[7.8113 * 2], [6.85333 * 2], [2.58656 * 2]])
p = p2 - p1
sq = math.sqrt(p[0][0] * p[0][0] + p[1][0] * p[1][0] + p[2][0] * p[2][0])
x1 = p[0][0] / sq
y1 = p[1][0] / sq
z1 = p[2][0] / sq
# 求出第一个轴的单位向量
v1 = np.array([x1, y1, z1])
print(v1)
# 构造出一个向量（0，0，1）
v = np.array([0, 0, 1])  # z轴
# 求出第二个轴的单位向量
v2 = np.array(np.cross(v, v1))  # y轴
# print(v2[0])
sq = math.sqrt(v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2])
x2 = v2[0] / sq
y2 = v2[1] / sq
z2 = v2[2] / sq
v2 = np.array([x2, y2, z2])
print(v2)
# # 求出第三个轴的单位向量
v3 = np.array(np.cross(v1, v2))
# sq = math.sqrt(v3[0] * v3[0] + v3[1] * v3[1] + v3[2] * v3[2])
# x3 = v3[0] / sq
# y3 = v3[1] / sq
# z3 = v3[2]/ sq
# v3 = np.array([x3,y3,z3])
# 从全局坐标系到当前坐标系的转换矩阵

print("转换矩阵")
Tru = np.array([[v1], [v2], [v3]])
print(Tru[0])
print(Tru[1])
print(Tru[2])
# 计算出三个欧拉角
print("输入欧拉角为：")
# xTheta=math.atan(z2/z3)
# yTheta = math.atan(-x3/math.sqrt(z2*z2+z3*z3))
# zTheta = math.atan(y1/ x1)
# print(xTheta,yTheta,zTheta)
vTheta = math.asin(-z1)
vphi = math.asin(y1 / math.cos(vTheta))
vpsi = math.acos(v3[2] / math.cos(vTheta))

print(vTheta, vphi, vpsi)