forked from OnScale/Engineering_Python
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathRotationFunction.py
More file actions
31 lines (26 loc) · 842 Bytes
/
RotationFunction.py
File metadata and controls
31 lines (26 loc) · 842 Bytes
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
# -*- coding: utf-8 -*-
"""
Created on Fri Jun 17 12:33:03 2016
@author: admingh
"""
import numpy as np
import math
def rotation_matrix(axis, theta):
"""
Return the rotation matrix associated with counterclockwise rotation about
the given axis by theta radians.
"""
axis = np.asarray(axis)
theta = np.asarray(theta)
axis = axis/math.sqrt(np.dot(axis, axis))
a = math.cos(theta/2.0)
b, c, d = -axis*math.sin(theta/2.0)
aa, bb, cc, dd = a*a, b*b, c*c, d*d
bc, ad, ac, ab, bd, cd = b*c, a*d, a*c, a*b, b*d, c*d
return np.array([[aa+bb-cc-dd, 2*(bc+ad), 2*(bd-ac)],
[2*(bc-ad), aa+cc-bb-dd, 2*(cd+ab)],
[2*(bd+ac), 2*(cd-ab), aa+dd-bb-cc]])
v = np.array([3, 5, 0], [1, 4, 5])
axis = [4, 4, 1]
theta = 1.2
print(np.dot(rotation_matrix(axis,theta), v))