问题描述
def rotatePoint(p1,p2,a):
if a == 0: return p2
x1,y1,z1 = p1
x2,y2,z2 = p2
d12 = math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1))
aP1P2 = math.atan2(z2 - z1,x2 - x1) # In radians,not degrees
aP1P3 = aP1P2 - math.radians(a)
x3 = x1 + d12 * math.cos(aP1P3)
y3 = y2# same as P1 an P2
z3 = z1 + d12 * math.sin(aP1P3)
p3 = (x3,y3,z3)
return p3
p1 = (0,0)
p2 = (0,20)
a = 45
print(rotatePoint(p1,a))
#Outputs:(14.142135623730951,14.14213562373095)
我该如何改变它，让它也可以绕 x 平面旋转？
编辑： 我查看了一个旋转矩阵，但我无法弄清楚。这是我得到的，我知道这是错误的。有什么问题：
def getRotationMatrix(a1,a2,a3):
sin = math.sin
cos = math.cos
xrot = cos(a2) * cos(a3) * cos(a1)*sin(a3)+sin(a1)*sin(a2)*cos(a3) * sin(a1)*sin(a3) - cos(a1)*sin(a2)*cos(a3)
yrot = -cos(a2)*sin(a3) * cos(a1)*cos(a3) - sin(a1)*sin(a2)*sin(a3) * sin(a1)*cos(a3) + cos(a1)*sin(a2)*sin(a3)
zrot = sin(a2) * -sin(a1)*cos(a2) * cos(a1)*cos(a2)
return [xrot,yrot,zrot]
解决方法
我正在关注 -
中介绍的转换细节https://en.wikipedia.org/wiki/Davenport_chained_rotations https://en.wikipedia.org/wiki/Rotation_matrix
def rotatePoint(p1,p2,a):
'''
https://en.wikipedia.org/wiki/Davenport_chained_rotations
https://en.wikipedia.org/wiki/Rotation_matrix
'''
# rotation in degrees
# convert to radians
alpha,beta,gamma=[math.radians(x) for x in a]
print(f'Rotation angles = {alpha},{beta},{gamma}')
# math functions alias
cos=math.cos
sin=math.sin
Rx_alpha=np.array([[1.0,0.0,0.0 ],[0.0,cos(alpha),-sin(alpha)],sin(alpha),cos(alpha) ]])
Ry_beta=np.array([[ cos(beta),sin(beta)],[ 0.0,1.0,0.0 ],[-sin(beta),cos(beta)]])
Rz_gamma=np.array([[cos(gamma),-sin(gamma),0.0],[sin(gamma),cos(gamma),1.0]])
R=np.matmul(np.matmul(Rz_gamma,Ry_beta),Rx_alpha)
print(f'Rotation matrix = {R}')
# calculate the vector about point p1,point_vector=np.array(p2)-np.array(p1)
point_vector_magnitude=np.linalg.norm(point_vector)
# normalized
point_vector=point_vector/point_vector_magnitude
print(f'point_vector={point_vector}')
print(f'point_vector_magnitude={point_vector_magnitude}')
rotated_vector=np.matmul(R,point_vector)*point_vector_magnitude
# calculate the transformed point p3
p3=np.array(p1)+rotated_vector
print(f'New Rotate point {p3}')
检查：
rotatePoint([0,0],[0,20],45])
输出：
Rotation angles = 0.0,0.7853981633974483
Rotation matrix = [[ 0.70710678 -0.70710678 0. ]
[ 0.70710678 0.70710678 0. ]
[ 0. 0. 1. ]]
point_vector=[0. 0. 1.]
point_vector_magnitude=20.0
unit_rotated_vector =[0. 0. 1.]
New Rotate point [ 0. 0. 20.]
rotatePoint([0,[45,0])
输出：
Rotation angles = 0.7853981633974483,0.0
Rotation matrix = [[ 1. 0. 0. ]
[ 0. 0.70710678 -0.70710678]
[ 0. 0.70710678 0.70710678]]
point_vector=[0. 0. 1.]
point_vector_magnitude=20.0
unit_rotated_vector =[ 0. -0.70710678 0.70710678]
New Rotate point [ 0. -14.14213562 14.14213562]
在我看来你是正确的。只有一个问题。 xrot、yrot 和 zrot 是分别包含三个组件的列表。所以删除乘号并用逗号替换并将它们放在一个列表中。下面是一个简单的实现。旋转的顺序是 important 先是 x 轴，然后是 y 轴，然后是 z 轴。在您的情况下可以，因为您希望围绕单个轴旋转。您可以通过将其他两个角度设置为零来使用以下功能。在进一步使用它之前，请通过在多种情况下进行测试来仔细检查此方法的结果。如果有错误，请告诉我。我只测试了两个简单的案例。
import math
def TranslateToOriginAndReturnUndoVector(x1,y1,z1):
return (x1,z1)
def getRotationMatrix(a1,a2,a3,x1,z1,x2,y2,z2):
## Step 1 translate to origin
x_translated,y_translated,z_translated = TranslateToOriginAndReturnUndoVector(x1,z1)
## Step 2 Calculate Rotation Matrix and new x,y,z w.r.t Step 1 values
sin = math.sin
cos = math.cos
xrot = [cos(a2) * cos(a3),cos(a1)*sin(a3)+sin(a1)*sin(a2)*cos(a3),sin(a1)*sin(a3) - cos(a1)*sin(a2)*cos(a3)]
yrot = [-cos(a2)*sin(a3),cos(a1)*cos(a3) - sin(a1)*sin(a2)*sin(a3),sin(a1)*cos(a3) + cos(a1)*sin(a2)*sin(a3)]
zrot = [sin(a2),-sin(a1)*cos(a2),cos(a1)*cos(a2)]
x_new = 0
y_new = 0
z_new = 0
for i in range(3):
x_new += xrot[i]*(x2 - x_translated)
y_new += yrot[i]*(y2 - y_translated)
z_new += zrot[i]*(z2 - z_translated)
x_new,y_new,z_new = round(x_new,3),round(y_new,round(z_new,3)
## Step 3,Undo Step 1
x_new,z_new = x_new + x_translated,y_new + y_translated,z_new + z_translated
## Return the new values
return (x_new,z_new)
print(getRotationMatrix(0,math.pi/4,20,20))
print(getRotationMatrix(0,10,20))
给出以下输出。
(0.0,20.0,28.284)
(10.0,24.142)