rotmat.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 #
3 # vector3 and rotation matrix classes
4 # This follows the conventions in the ArduPilot code,
5 # and is essentially a python version of the AP_Math library
6 #
7 # Andrew Tridgell, March 2012
8 #
9 # This library is free software; you can redistribute it and/or modify it
11 # Free Software Foundation; either version 2.1 of the License, or (at your
12 # option) any later version.
13 #
14 # This library is distributed in the hope that it will be useful, but WITHOUT
15 # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
16 # FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
17 # for more details.
18 #
19 # You should have received a copy of the GNU Lesser General Public License
20 # along with this library; if not, write to the Free Software Foundation,
21 # Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
22
23 '''rotation matrix class
24 '''
25 from __future__ import print_function
26 from builtins import range
27 from builtins import object
28
29 from math import sin, cos, sqrt, asin, atan2, pi, radians, acos, degrees
30
31 class Vector3(object):
32  '''a vector'''
33  def __init__(self, x=None, y=None, z=None):
34  if x is not None and y is not None and z is not None:
35  self.x = float(x)
36  self.y = float(y)
37  self.z = float(z)
38  elif x is not None and len(x) == 3:
39  self.x = float(x[0])
40  self.y = float(x[1])
41  self.z = float(x[2])
42  elif x is not None:
44  else:
45  self.x = float(0)
46  self.y = float(0)
47  self.z = float(0)
48
49  def __repr__(self):
50  return 'Vector3(%.2f, %.2f, %.2f)' % (self.x,
51  self.y,
52  self.z)
53
54  def __eq__(self, v):
55  return self.x == v.x and self.y == v.y and self.z == v.z
56
57  def __ne__(self, v):
58  return not self == v
59
60  def close(self, v, tol=1e-7):
61  return abs(self.x - v.x) < tol and \
62  abs(self.y - v.y) < tol and \
63  abs(self.z - v.z) < tol
64
66  return Vector3(self.x + v.x,
67  self.y + v.y,
68  self.z + v.z)
69
71
72  def __sub__(self, v):
73  return Vector3(self.x - v.x,
74  self.y - v.y,
75  self.z - v.z)
76
77  def __neg__(self):
78  return Vector3(-self.x, -self.y, -self.z)
79
80  def __rsub__(self, v):
81  return Vector3(v.x - self.x,
82  v.y - self.y,
83  v.z - self.z)
84
85  def __mul__(self, v):
86  if isinstance(v, Vector3):
87  '''dot product'''
88  return self.x*v.x + self.y*v.y + self.z*v.z
89  return Vector3(self.x * v,
90  self.y * v,
91  self.z * v)
92
93  __rmul__ = __mul__
94
95  def __div__(self, v):
96  return Vector3(self.x / v,
97  self.y / v,
98  self.z / v)
99
100  def __mod__(self, v):
101  '''cross product'''
102  return Vector3(self.y*v.z - self.z*v.y,
103  self.z*v.x - self.x*v.z,
104  self.x*v.y - self.y*v.x)
105
106  def __copy__(self):
107  return Vector3(self.x, self.y, self.z)
108
109  copy = __copy__
110
111  def length(self):
112  return sqrt(self.x**2 + self.y**2 + self.z**2)
113
114  def zero(self):
115  self.x = self.y = self.z = 0
116
117  def angle(self, v):
118  '''return the angle between this vector and another vector'''
119  return acos((self * v) / (self.length() * v.length()))
120
121  def normalized(self):
122  return self.__div__(self.length())
123
124  def normalize(self):
125  v = self.normalized()
126  self.x = v.x
127  self.y = v.y
128  self.z = v.z
129
130 class Matrix3(object):
131  '''a 3x3 matrix, intended as a rotation matrix'''
132  def __init__(self, a=None, b=None, c=None):
133  if a is not None and b is not None and c is not None:
134  self.a = a.copy()
135  self.b = b.copy()
136  self.c = c.copy()
137  else:
138  self.identity()
139
140  def __repr__(self):
141  return 'Matrix3((%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f))' % (
142  self.a.x, self.a.y, self.a.z,
143  self.b.x, self.b.y, self.b.z,
144  self.c.x, self.c.y, self.c.z)
145
146  def identity(self):
147  self.a = Vector3(1,0,0)
148  self.b = Vector3(0,1,0)
149  self.c = Vector3(0,0,1)
150
151  def transposed(self):
152  return Matrix3(Vector3(self.a.x, self.b.x, self.c.x),
153  Vector3(self.a.y, self.b.y, self.c.y),
154  Vector3(self.a.z, self.b.z, self.c.z))
155
156
157  def from_euler(self, roll, pitch, yaw):
158  '''fill the matrix from Euler angles in radians'''
159  cp = cos(pitch)
160  sp = sin(pitch)
161  sr = sin(roll)
162  cr = cos(roll)
163  sy = sin(yaw)
164  cy = cos(yaw)
165
166  self.a.x = cp * cy
167  self.a.y = (sr * sp * cy) - (cr * sy)
168  self.a.z = (cr * sp * cy) + (sr * sy)
169  self.b.x = cp * sy
170  self.b.y = (sr * sp * sy) + (cr * cy)
171  self.b.z = (cr * sp * sy) - (sr * cy)
172  self.c.x = -sp
173  self.c.y = sr * cp
174  self.c.z = cr * cp
175
176
177  def to_euler(self):
178  '''find Euler angles (321 convention) for the matrix'''
179  if self.c.x >= 1.0:
180  pitch = pi
181  elif self.c.x <= -1.0:
182  pitch = -pi
183  else:
184  pitch = -asin(self.c.x)
185  roll = atan2(self.c.y, self.c.z)
186  yaw = atan2(self.b.x, self.a.x)
187  return (roll, pitch, yaw)
188
189
190  def to_euler312(self):
191  '''find Euler angles (312 convention) for the matrix.
193  '''
194  T21 = self.a.y
195  T22 = self.b.y
196  T23 = self.c.y
197  T13 = self.c.x
198  T33 = self.c.z
199  yaw = atan2(-T21, T22)
200  roll = asin(T23)
201  pitch = atan2(-T13, T33)
202  return (roll, pitch, yaw)
203
204  def from_euler312(self, roll, pitch, yaw):
205  '''fill the matrix from Euler angles in radians in 312 convention'''
206  c3 = cos(pitch)
207  s3 = sin(pitch)
208  s2 = sin(roll)
209  c2 = cos(roll)
210  s1 = sin(yaw)
211  c1 = cos(yaw)
212
213  self.a.x = c1 * c3 - s1 * s2 * s3
214  self.b.y = c1 * c2
215  self.c.z = c3 * c2
216  self.a.y = -c2*s1
217  self.a.z = s3*c1 + c3*s2*s1
218  self.b.x = c3*s1 + s3*s2*c1
219  self.b.z = s1*s3 - s2*c1*c3
220  self.c.x = -s3*c2
221  self.c.y = s2
222
224  return Matrix3(self.a + m.a, self.b + m.b, self.c + m.c)
225
227
228  def __sub__(self, m):
229  return Matrix3(self.a - m.a, self.b - m.b, self.c - m.c)
230
231  def __rsub__(self, m):
232  return Matrix3(m.a - self.a, m.b - self.b, m.c - self.c)
233
234  def __eq__(self, m):
235  return self.a == m.a and self.b == m.b and self.c == m.c
236
237  def __ne__(self, m):
238  return not self == m
239
240  def __mul__(self, other):
241  if isinstance(other, Vector3):
242  v = other
243  return Vector3(self.a.x * v.x + self.a.y * v.y + self.a.z * v.z,
244  self.b.x * v.x + self.b.y * v.y + self.b.z * v.z,
245  self.c.x * v.x + self.c.y * v.y + self.c.z * v.z)
246  elif isinstance(other, Matrix3):
247  m = other
248  return Matrix3(Vector3(self.a.x * m.a.x + self.a.y * m.b.x + self.a.z * m.c.x,
249  self.a.x * m.a.y + self.a.y * m.b.y + self.a.z * m.c.y,
250  self.a.x * m.a.z + self.a.y * m.b.z + self.a.z * m.c.z),
251  Vector3(self.b.x * m.a.x + self.b.y * m.b.x + self.b.z * m.c.x,
252  self.b.x * m.a.y + self.b.y * m.b.y + self.b.z * m.c.y,
253  self.b.x * m.a.z + self.b.y * m.b.z + self.b.z * m.c.z),
254  Vector3(self.c.x * m.a.x + self.c.y * m.b.x + self.c.z * m.c.x,
255  self.c.x * m.a.y + self.c.y * m.b.y + self.c.z * m.c.y,
256  self.c.x * m.a.z + self.c.y * m.b.z + self.c.z * m.c.z))
257  v = other
258  return Matrix3(self.a * v, self.b * v, self.c * v)
259
260  def __div__(self, v):
261  return Matrix3(self.a / v, self.b / v, self.c / v)
262
263  def __neg__(self):
264  return Matrix3(-self.a, -self.b, -self.c)
265
266  def __copy__(self):
267  return Matrix3(self.a, self.b, self.c)
268
269  copy = __copy__
270
271  def rotate(self, g):
272  '''rotate the matrix by a given amount on 3 axes,
273  where g is a vector of delta angles. Used
274  with DCM updates in mavextra.py'''
275  temp_matrix = Matrix3()
276  a = self.a
277  b = self.b
278  c = self.c
279  temp_matrix.a.x = a.y * g.z - a.z * g.y
280  temp_matrix.a.y = a.z * g.x - a.x * g.z
281  temp_matrix.a.z = a.x * g.y - a.y * g.x
282  temp_matrix.b.x = b.y * g.z - b.z * g.y
283  temp_matrix.b.y = b.z * g.x - b.x * g.z
284  temp_matrix.b.z = b.x * g.y - b.y * g.x
285  temp_matrix.c.x = c.y * g.z - c.z * g.y
286  temp_matrix.c.y = c.z * g.x - c.x * g.z
287  temp_matrix.c.z = c.x * g.y - c.y * g.x
288  self.a += temp_matrix.a
289  self.b += temp_matrix.b
290  self.c += temp_matrix.c
291
292  def normalize(self):
293  '''re-normalise a rotation matrix'''
294  error = self.a * self.b
295  t0 = self.a - (self.b * (0.5 * error))
296  t1 = self.b - (self.a * (0.5 * error))
297  t2 = t0 % t1
298  self.a = t0 * (1.0 / t0.length())
299  self.b = t1 * (1.0 / t1.length())
300  self.c = t2 * (1.0 / t2.length())
301
302  def trace(self):
303  '''the trace of the matrix'''
304  return self.a.x + self.b.y + self.c.z
305
306  def from_axis_angle(self, axis, angle):
307  '''create a rotation matrix from axis and angle'''
308  ux = axis.x
309  uy = axis.y
310  uz = axis.z
311  ct = cos(angle)
312  st = sin(angle)
313  self.a.x = ct + (1-ct) * ux**2
314  self.a.y = ux*uy*(1-ct) - uz*st
315  self.a.z = ux*uz*(1-ct) + uy*st
316  self.b.x = uy*ux*(1-ct) + uz*st
317  self.b.y = ct + (1-ct) * uy**2
318  self.b.z = uy*uz*(1-ct) - ux*st
319  self.c.x = uz*ux*(1-ct) - uy*st
320  self.c.y = uz*uy*(1-ct) + ux*st
321  self.c.z = ct + (1-ct) * uz**2
322
323
324  def from_two_vectors(self, vec1, vec2):
325  '''get a rotation matrix from two vectors.
326  This returns a rotation matrix which when applied to vec1
327  will produce a vector pointing in the same direction as vec2'''
328  angle = vec1.angle(vec2)
329  cross = vec1 % vec2
330  if cross.length() == 0:
331  # the two vectors are colinear
332  return self.from_euler(0,0,angle)
333  cross.normalize()
334  return self.from_axis_angle(cross, angle)
335
336  def close(self, m, tol=1e-7):
337  return self.a.close(m.a, tol) and self.b.close(m.b, tol) and self.c.close(m.c, tol)
338
339 class Plane(object):
340  '''a plane in 3 space, defined by a point and a vector normal'''
341  def __init__(self, point=None, normal=None):
342  if point is None:
343  point = Vector3(0,0,0)
344  if normal is None:
345  normal = Vector3(0, 0, 1)
346  self.point = point
347  self.normal = normal
348
349 class Line(object):
350  '''a line in 3 space, defined by a point and a vector'''
351  def __init__(self, point=None, vector=None):
352  if point is None:
353  point = Vector3(0,0,0)
354  if vector is None:
355  vector = Vector3(0, 0, 1)
356  self.point = point
357  self.vector = vector
358
359  def plane_intersection(self, plane, forward_only=False):
360  '''return point where line intersects with a plane'''
361  l_dot_n = self.vector * plane.normal
362  if l_dot_n == 0.0:
363  # line is parallel to the plane
364  return None
365  d = ((plane.point - self.point) * plane.normal) / l_dot_n
366  if forward_only and d < 0:
367  return None
368  return (self.vector * d) + self.point
369