test_rotmat.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2
3
4 """
5 Unit tests for the rotmat library
6 """
7
8 from __future__ import absolute_import, division, print_function
9 from math import radians, degrees
10 import unittest
11 import random
12 import numpy as np
13
14 from pymavlink.rotmat import Vector3, Matrix3, Plane, Line
15
16 class VectorTest(unittest.TestCase):
17
18  """
19  Class to test Vector3
20  """
21
22  def __init__(self, *args, **kwargs):
23  """Constructor, set up some data that is reused in many tests"""
24  super(VectorTest, self).__init__(*args, **kwargs)
25
26
27  def test_constructor(self):
28  """Test the constructor functionality"""
29  v1 = Vector3(1, 0.2, -3)
30  v2 = Vector3([1, 0.2, -3])
31  v3 = Vector3([1, 0.3, -3])
32
33  assert v1 == v2
34  assert v1 != v3
35  assert str(v1) == "Vector3(1.00, 0.20, -3.00)"
36
37
38  def test_maths(self):
39  """Test simple maths"""
40  v1 = Vector3(1, 2, -3)
41  v2 = Vector3(1, 3, 3)
42
43  assert v1 + v2 == Vector3(2, 5, 0)
44  assert v1 - v2 == Vector3(0, -1, -6)
45  assert (v1 * 3) == Vector3(3, 6, -9)
46  assert v1 * v2 == -2
47
48  assert v1 % v2 == Vector3(15.00, -6.00, 1.00)
49  np.testing.assert_almost_equal(v2.length(), 4.358898943540674)
50  assert v2.normalized().close(Vector3(0.23, 0.69, 0.69), tol=1e-2)
51  np.testing.assert_almost_equal(v1.angle(v2), 1.693733631245806)
52
53
54 class MatrixTest(unittest.TestCase):
55
56  """
57  Class to test Matrix3
58  """
59
60  def __init__(self, *args, **kwargs):
61  """Constructor, set up some data that is reused in many tests"""
62  super(MatrixTest, self).__init__(*args, **kwargs)
63
64  def test_constructor(self):
65  """Test the constructor functionality"""
66  m1 = Matrix3(Vector3(1, 0, 0), Vector3(1, 5, 0), Vector3(1, 0, -7))
67  m2 = Matrix3()
68
69  assert str(m1) == 'Matrix3((1.00, 0.00, 0.00), (1.00, 5.00, 0.00), (1.00, 0.00, -7.00))'
70  assert str(m2) == 'Matrix3((1.00, 0.00, 0.00), (0.00, 1.00, 0.00), (0.00, 0.00, 1.00))'
71
72  def test_maths(self):
73  m1 = Matrix3(Vector3(1, 0, 0), Vector3(1, 5, 0), Vector3(1, 0, -7))
74  m2 = Matrix3()
75
76  assert m1 + m2 == Matrix3(Vector3(2, 0, 0), Vector3(1, 6, 0), Vector3(1, 0, -6))
77  assert m1 - m2 == Matrix3(Vector3(0, 0, 0), Vector3(1, 4, 0), Vector3(1, 0, -8))
78  assert m1 * 3 == Matrix3(Vector3(3, 0, 0), Vector3(3, 15, 0), Vector3(3, 0, -21))
79  assert m1 * m1 == Matrix3(Vector3(1, 0, 0), Vector3(6, 25, 0), Vector3(-6, 0, 49))
80  assert m1.transposed() == Matrix3(Vector3(1, 1, 1), Vector3(0, 5, 0), Vector3(0, 0, -7))
81
82  def test_euler(self):
83  '''check that from_euler() and to_euler() are consistent'''
84  m = Matrix3()
85  for r in range(-179, 179, 10):
86  for p in range(-89, 89, 10):
87  for y in range(-179, 179, 10):
89  (r2, p2, y2) = m.to_euler()
90  v1 = Vector3(r, p, y)
91  v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
92  diff = v1 - v2
93  assert diff.length() < 1.0e-12
94
95  def test_euler312(self):
96  '''check that from_euler312() and to_euler312() are consistent'''
97  m = Matrix3()
98  for r in range(-89, 89, 10):
99  for p in range(-179, 179, 10):
100  for y in range(-179, 179, 10):
102  (r2, p2, y2) = m.to_euler312()
103  v1 = Vector3(r, p, y)
104  v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
105  diff = v1 - v2
106  assert diff.length() < 1.0e-12
107
108  def test_matrixops(self):
109  m1 = Matrix3(Vector3(1, 0, 0), Vector3(1, 5, 0), Vector3(1, 0, -7))
110
111  m1.normalize()
112  print(m1)
113  assert m1.close(Matrix3(Vector3(0.2, -0.98, 0), Vector3(0.1, 1, 0), Vector3(0, 0, 1)), tol=1e-2)
114  np.testing.assert_almost_equal(m1.trace(), 2.19115332535)
115
116  m1.rotate(Vector3(0.2,-0.98,0))
117  assert m1.close(Matrix3(Vector3(0.2,-0.98,0), Vector3(0.1,1,-0.3), Vector3(0.98,0.2,1)), tol=1e-2)
118
119  def test_axisangle(self):
120  axis = Vector3(0, 1, 0)
122
123  m1 = Matrix3()
124  m1.from_axis_angle(axis, angle)
125  #print(m1)
126  assert m1.close(Matrix3(Vector3(0.71, 0.00, 0.71),
127  Vector3(0.00, 1.00, 0.00),
128  Vector3(-0.71, 0.00, 0.71)), tol=1e-2)
129
130  def test_two_vectors(self):
131  '''test the from_two_vectors() method'''
132  for i in range(100):
133  v1 = Vector3(1, 0.2, -3)
134  v2 = Vector3(random.uniform(-5, 5), random.uniform(-5, 5), random.uniform(-5, 5))
135  m = Matrix3()
136  m.from_two_vectors(v1, v2)
137  v3 = m * v1
138  diff = v3.normalized() - v2.normalized()
139  (r, p, y) = m.to_euler()
140  assert diff.length() < 0.001
141
142
143 class LinePlaneTest(unittest.TestCase):
144
145  """
146  Class to test Line and Plane classes
147  """
148
149  def __init__(self, *args, **kwargs):
150  """Constructor, set up some data that is reused in many tests"""
151  super(LinePlaneTest, self).__init__(*args, **kwargs)
152
153  def test_plane(self):
154  '''testing line/plane intersection'''
155  plane = Plane(Vector3(0, 0, 0), Vector3(0, 0, 1))
156  line = Line(Vector3(0, 0, 100), Vector3(10, 10, -90))
157  p = line.plane_intersection(plane)
158  assert p.close(Vector3(11.11, 11.11, 0.00), tol=1e-2)
159
160 if __name__ == '__main__':
161  unittest.main()
def test_constructor(self)
Definition: test_rotmat.py:64
def __init__(self, args, kwargs)
Definition: test_rotmat.py:22
def test_two_vectors(self)
Definition: test_rotmat.py:130
def test_constructor(self)
Definition: test_rotmat.py:27
def __init__(self, args, kwargs)
Definition: test_rotmat.py:149
def __init__(self, args, kwargs)
Definition: test_rotmat.py:60