-
Notifications
You must be signed in to change notification settings - Fork 1
/
rotation.py
39 lines (32 loc) · 1.27 KB
/
rotation.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
# http://stackoverflow.com/a/12261243
import numpy as np
from math import cos, sin, sqrt
from scipy import weave
def rotation_matrix_weave(axis, theta, mat):
support = "#include <math.h>"
code = """
double x = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
double a = cos(theta / 2.0);
double b = -(axis[0] / x) * sin(theta / 2.0);
double c = -(axis[1] / x) * sin(theta / 2.0);
double d = -(axis[2] / x) * sin(theta / 2.0);
mat[0] = a*a + b*b - c*c - d*d;
mat[1] = 2 * (b*c - a*d);
mat[2] = 2 * (b*d + a*c);
mat[3*1 + 0] = 2*(b*c+a*d);
mat[3*1 + 1] = a*a+c*c-b*b-d*d;
mat[3*1 + 2] = 2*(c*d-a*b);
mat[3*2 + 0] = 2*(b*d-a*c);
mat[3*2 + 1] = 2*(c*d+a*b);
mat[3*2 + 2] = a*a+d*d-b*b-c*c;
"""
weave.inline(code, ['axis', 'theta', 'mat'], support_code = support, libraries = ['m'])
return mat
def rotation_matrix(axis, theta, mat):
axis = np.array(axis)
axis = axis/sqrt(np.dot(axis, axis))
a = cos(theta/2.)
b, c, d = -axis*sin(theta/2.)
return np.array([[a*a+b*b-c*c-d*d, 2*(b*c-a*d), 2*(b*d+a*c)],
[2*(b*c+a*d), a*a+c*c-b*b-d*d, 2*(c*d-a*b)],
[2*(b*d-a*c), 2*(c*d+a*b), a*a+d*d-b*b-c*c]])