|
import numpy as np |
|
import math |
|
import scipy.linalg as linalg |
|
|
|
|
|
def rotate_mat(axis, radian): |
|
|
|
a = np.cross(np.eye(3), axis / linalg.norm(axis) * radian) |
|
|
|
rot_matrix = linalg.expm(a) |
|
|
|
return rot_matrix |
|
|
|
def aaa2mat(axis, sin, cos): |
|
i = np.eye(3) |
|
nnt = np.dot(axis.T, axis) |
|
s = np.asarray([[0, -axis[0,2], axis[0,1]], |
|
[axis[0,2], 0, -axis[0,0]], |
|
[-axis[0,1], axis[0,0], 0]]) |
|
r = cos * i + (1-cos)*nnt +sin * s |
|
return r |
|
|
|
rand_axis = np.asarray([[1,0,0]]) |
|
|
|
r = math.pi/2 |
|
|
|
rot_matrix = rotate_mat(rand_axis, r) |
|
r2 = aaa2mat(rand_axis, np.sin(r), np.cos(r)) |
|
print(rot_matrix) |
