| | 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) |
