python - 在 Python 中旋转 3D 点

Question

我目前正在尝试旋转四个点以使其位置与另一组四个点相匹配。我使用 3D 旋转矩阵来执行此操作。但我得到以下问题：

旋转后点之间的距离会发生变化，我不明白为什么。

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]) 

标签： pythonnumpygeometry