def rotation_matrix(u, theta):
'''Return matrix that implements the rotation around the vector :math:`u`
by the angle :math:`\\theta`, cf.
https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle.
:param u: rotation vector
:param theta: rotation angle
'''
# Cross-product matrix.
cpm = numpy.array([[0.0, -u[2], u[1]],
[u[2], 0.0, -u[0]],
[-u[1], u[0], 0.0]])
c = numpy.cos(theta)
s = numpy.sin(theta)
R = numpy.eye(3) * c \
+ s * cpm \
+ (1.0 - c) * numpy.outer(u, u)
return R
评论列表
文章目录