在Python中旋转三维点

2024-10-04 11:25:34 发布

您现在位置:Python中文网/ 问答频道 /正文

我目前正在尝试旋转四个点,使它们的位置与另一组四个点相匹配。我用一个三维旋转矩阵。但我有以下问题:

点之间的距离在旋转后改变,我不明白为什么。你知道吗

squared_dist = np.sum((tarll-tarul)**2, axis=0)
dist2 = np.sqrt(squared_dist)
print(dist2)

theta = -0.46 

tarlr[0] = tarlr[0] * math.cos(theta) - tarlr[1] * math.sin(theta)
tarlr[1] = tarlr[0] * math.sin(theta) + tarlr[1] * math.cos(theta)
tarlr[2] = tarlr[2]

tarll[0] = tarll[0] * math.cos(theta) - tarll[1] * math.sin(theta)
tarll[1] = tarll[0] * math.sin(theta) + tarll[1] * math.cos(theta)
tarll[2] = tarll[2]

tarur[0] = tarur[0] * math.cos(theta) - tarur[1] * math.sin(theta)
tarur[1] = tarur[0] * math.sin(theta) + tarur[1] * math.cos(theta)
tarur[2] = tarur[2]

tarul[0] = tarul[0] * math.cos(theta) - tarul[1] * math.sin(theta)
tarul[1] = tarul[0] * math.sin(theta) + tarul[1] * math.cos(theta)
tarul[2] = tarul[2]

squared_dist = np.sum((tarll-tarul)**2, axis=0)
dist3 = np.sqrt(squared_dist)
print(dist3)

要点是:

#Tar position
tarlr = np.array([2.5679,3.5465,3.9583]) 
tarur = np.array([5.0263,-0.3365,3.9411]) 
tarul = np.array([1.1462,-2.7898,3.9491]) 
tarll = np.array([-1.3130,1.0835,3.9638]) 

#old  Parameter
tarllOld = np.array([-2.4708,2.3395,4.101]) 
tarlrOld = np.array([2.1218,2.5116,4.0952]) 
tarurOld = np.array([2.2889,-2.0805,4.0774]) 
tarulOld = np.array([-2.2992,-2.2462,4.0859]) 

Tags: distnpmathsincosarraysumaxis