-
Notifications
You must be signed in to change notification settings - Fork 5
/
rotations2d.py
145 lines (107 loc) · 3.82 KB
/
rotations2d.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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
r"""
A set of rotation utilites for manipulating 2-dimensional vectors
"""
from __future__ import (division, print_function, absolute_import,
unicode_literals)
import numpy as np
from rotations.vector_utilities import (elementwise_dot, elementwise_norm,
normalized_vectors, angles_between_list_of_vectors)
__all__=['rotation_matrices_from_angles',
'rotation_matrices_from_vectors',
'rotation_matrices_from_basis']
__author__ = ['Duncan Campbell', 'Andrew Hearin']
def rotation_matrices_from_angles(angles):
r"""
Calculate a collection of rotation matrices defined by
an input collection of rotation angles and rotation axes.
Parameters
----------
angles : ndarray
Numpy array of shape (npts, ) storing a collection of rotation angles
Returns
-------
matrices : ndarray
Numpy array of shape (npts, 2, 2) storing a collection of rotation matrices
Examples
--------
>>> npts = int(1e4)
>>> angles = np.random.uniform(-np.pi/2., np.pi/2., npts)
>>> rotation_matrices = rotation_matrices_from_angles(angles, directions)
Notes
-----
The function `rotate_vector_collection` can be used to efficiently
apply the returned collection of matrices to a collection of 2d vectors
"""
angles = np.atleast_1d(angles)
npts = len(angles)
sina = np.sin(angles)
cosa = np.cos(angles)
R = np.zeros((npts, 2, 2))
R[:, 0, 0] = cosa
R[:, 1, 1] = cosa
R[:, 0, 1] = -sina
R[:, 1, 0] = sina
return R
def rotation_matrices_from_vectors(v0, v1):
r"""
Calculate a collection of rotation matrices defined by the unique
transformation rotating v1 into v2.
Parameters
----------
v0 : ndarray
Numpy array of shape (npts, 2) storing a collection of initial vector orientations.
Note that the normalization of `v0` will be ignored.
v1 : ndarray
Numpy array of shape (npts, 2) storing a collection of final vectors.
Note that the normalization of `v1` will be ignored.
Returns
-------
matrices : ndarray
Numpy array of shape (npts, 2, 2) rotating each v0 into the corresponding v1
Examples
--------
>>> npts = int(1e4)
>>> v0 = np.random.random((npts, 2))
>>> v1 = np.random.random((npts, 2))
>>> rotation_matrices = rotation_matrices_from_vectors(v0, v1)
Notes
-----
The function `rotate_vector_collection` can be used to efficiently
apply the returned collection of matrices to a collection of 2d vectors
"""
v0 = normalized_vectors(v0)
v1 = normalized_vectors(v1)
# use the atan2 function to get the direction of rotation right
angles = np.arctan2(v0[:,0], v0[:,1])-np.arctan2(v1[:,0],v1[:,1])
return rotation_matrices_from_angles(angles)
def rotation_matrices_from_basis(ux, uy):
"""
Calculate a collection of rotation matrices defined by an input collection
of basis vectors.
Parameters
----------
ux : array_like
Numpy array of shape (npts, 2) storing a collection of unit vectors
uy : array_like
Numpy array of shape (npts, 2) storing a collection of unit vectors
Returns
-------
matrices : ndarray
Numpy array of shape (npts, 2, 2) storing a collection of rotation matrices
"""
N = np.shape(ux)[0]
# assume initial unit vectors are the standard ones
ex = np.array([1.0, 0.0]*N).reshape(N, 2)
ey = np.array([0.0, 1.0]*N).reshape(N, 2)
ux = normalized_vectors(ux)
uy = normalized_vectors(uy)
r_11 = elementwise_dot(ex, ux)
r_12 = elementwise_dot(ex, uy)
r_21 = elementwise_dot(ey, ux)
r_22 = elementwise_dot(ey, uy)
r = np.zeros((N, 2, 2))
r[:,0,0] = r_11
r[:,0,1] = r_12
r[:,1,0] = r_21
r[:,1,1] = r_22
return r