问题
在三维空间中,计算一个向量 v v v(或点 v v v)绕另一个向量 u u u旋转 θ \theta θ角后的坐标。
方法
罗德里格旋转公式(Rodrigues’ Rotation Formula):
v
′
=
v
c
o
s
θ
+
u
×
v
s
i
n
θ
+
(
u
⋅
v
)
u
(
1
−
c
o
s
θ
)
v' = v cos \theta + u \times v sin \theta + (u \cdot v) u ( 1 - cos \theta)
v′=vcosθ+u×vsinθ+(u⋅v)u(1−cosθ)
公式推导可见 旋转之二 - 三维空间中的旋转:罗德里格旋转公式
示例代码
下面的代码是计算点 v v v绕向量 u u u旋转360°之后的点集。
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
def RodriguesRotate(v:np.ndarray,u:np.ndarray,theta:float)->np.ndarray:
'''向量v绕向量u旋转角度θ,得到新的向量P_new
罗德里格斯旋转公式:v' = vcosθ + (u×v)sinθ + (u·v)u(1-cosθ)
args:
v:向量,维度为(3,)
u:作为旋转轴的向量,维度为(3,)
theta:旋转角度θ,此处默认为角度值
returns:
v_new:旋转后得到的向量,维度为(3,)
'''
u = u/np.linalg.norm(u) # 计算单位向量
sin_theta = np.sin(theta*np.pi/180)
cos_theta = np.cos(theta*np.pi/180)
v_new = v*cos_theta + np.cross(u,v)*sin_theta + np.dot(u,v)*u*(1-cos_theta)
return v_new
v = np.array([1.1,1.5,2])
u = np.array([1,2,3])
ax = plt.gca(projection='3d')
ax.set_xlim([0,3])
ax.set_ylim([0,3])
ax.set_zlim([0,3])
ax.quiver(0,0,0,u[0],u[1],u[2])
V = np.zeros([360,3])
V[0] = v
for theta in range(1,360):
V[theta]= RodriguesRotate(v,u,theta)
ax.scatter(V[:,0],V[:,1],V[:,2],c='cyan')
plt.show()