三维空间的旋转矩阵

import mathimport numpydef cross(a, b): return (a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0])def dot(a, b): return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]def normalize(a): a = numpy.array(a) return numpy.sqrt(numpy.sum(numpy.power(a, 2)))def cal_rotate_matrix(a, b): rot_axis = cross(b, a) rot_angle = math.acos(dot(a, b) / normalize(a) / normalize(b)) norm = normalize(rot_axis) rot_mat = numpy.zeros((3, 3), dtype = "float32") rot_axis = (rot_axis[0] / norm, rot_axis[1] / norm, rot_axis[2] / norm) rot_mat[0, 0] = math.cos(rot_angle) + rot_axis[0] * rot_axis[0] * (1 - math.cos(rot_angle)) rot_mat[0, 1] = rot_axis[0] * rot_axis[1] * (1 - math.cos(rot_angle)) - rot_axis[2] * math.sin(rot_angle) rot_mat[0, 2] = rot_axis[1] * math.sin(rot_angle) + rot_axis[0] * rot_axis[2] * (1 - math.cos(rot_angle)) rot_mat[1, 0] = rot_axis[2] * math.sin(rot_angle) + rot_axis[0] * rot_axis[1] * (1 - math.cos(rot_angle)) rot_mat[1, 1] = math.cos(rot_angle) + rot_axis[1] * rot_axis[1] * (1 - math.cos(rot_angle)) rot_mat[1, 2] = -rot_axis[0] * math.sin(rot_angle) + rot_axis[1] * rot_axis[2] * (1 - math.cos(rot_angle)) rot_mat[2, 0] = -rot_axis[1] * math.sin(rot_angle) + rot_axis[0] * rot_axis[2] * (1 - math.cos(rot_angle)) rot_mat[2, 1] = rot_axis[0] * math.sin(rot_angle) + rot_axis[1] * rot_axis[2] * (1 - math.cos(rot_angle)) rot_mat[2, 2] = math.cos(rot_angle) + rot_axis[2] * rot_axis[2] * (1 - math.cos(rot_angle)) return numpy.matrix(rot_mat)if __name__ == '__main__': a = (-0.006576016845720566, 0.20515224329972243, 0.011860567926381188) b = (0, 0.2056, 0) rot_mat = cal_rotate_matrix(a, b) print b print numpy.array(a) * rot_mat