mport sympy as sp
import math
from sympy import *
import time
import matplotlib.pyplot as plt
def get_R(a_x, a_y, a_z):
#a_x, a_y, a_z = sp.symbols('a_x a_y a_z')
#x, y , z = sp.symbols('x y z')
#lis = np.array(lis).reshape(-1,3)
# li = Matrix(lis).T
Rx = [[1, 0, 0],
[0, sp.cos(a_x), -sp.sin(a_x)],
[0, sp.sin(a_x), sp.cos(a_x)]]
Rx = Matrix(Rx)
#print(Rx)
Ry = [[sp.cos(a_y), 0, sp.sin(a_y)],
[0,1,0],
[-1*sp.sin(a_y),0, sp.cos(a_y)]]
Ry = Matrix(Ry)
Rz = [[sp.cos(a_z), -sp.sin(a_z),0],
[sp.sin(a_z), sp.cos(a_z), 0],
[0, 0, 1]]
Rz = Matrix(Rz)
#
R = Rz*Ry*Rx
return R
vector = [[1,2,3],[2,3,-3]]
def get_vertical_vector(vector):
a, b = sp.symbols('a, b')
vv = np.array([a,b,1])
v0 = np.array(vector[0])
v1 = np.array(vector[1])
r0 = np.dot(v0,vv)
r1 = np.dot(v1,vv)
#print(ab_dict[a])
ab_dict = sp.solve([r0,r1],[a,b]) # get a,b values by function group [r0, r1]
#print(sp.solve([r0,r1],[a,b]))
v_v = [ab_dict[a],ab_dict[b],1]
#print(v_v)
return v_v
vv = get_vertical_vector(vector)
def get_rotate_mat():
a_x, a_y, a_z = sp.symbols('a_x a_y a_z')
R = get_R(a_x, a_y, a_z)
#print(R, Matrix(vv))
len_vv = math.sqrt(vv[0]**2+vv[1]**2+vv[2]**2)
ff = R*Matrix(vv) - Matrix([0,0,len_vv])
angls = sp.solve([ff[0],ff[1],ff[2]],[a_x, a_y, a_z])
print(angls)
get_rotate_mat()
测试symply
最新推荐文章于 2022-10-11 22:51:33 发布