rotmat: added more euler 321 and 312 functions

rotmat.py

@@ -338,6 +338,51 @@ def rotate(self, g):
         self.b += temp_matrix.b
         self.c += temp_matrix.c
 
+    def rotate_yaw(self, yrad):
+        '''rotate the matrix by a given amount in-place on yaw axis'''
+        m2 = Matrix3()
+        m2.from_euler(0,0,yrad)
+        m2 = self * m2
+        self.a = m2.a
+        self.b = m2.b
+        self.c = m2.c
+
+    def rotate_pitch(self, prad):
+        '''rotate the matrix by a given amount in-place on pitch axis'''
+        m2 = Matrix3()
+        m2.from_euler(0,prad,0)
+        m2 = self * m2
+        self.a = m2.a
+        self.b = m2.b
+        self.c = m2.c
+
+    def rotate_roll(self, rrad):
+        '''rotate the matrix by a given amount in-place on roll axis'''
+        m2 = Matrix3()
+        m2.from_euler(rrad,0,0)
+        m2 = self * m2
+        self.a = m2.a
+        self.b = m2.b
+        self.c = m2.c
+
+    def rotate_321(self, ryad, prad, yrad):
+        '''rotate the matrix by a given amount in-place on 3 axes with 321 ordering'''
+        m2 = Matrix3()
+        m2.from_euler(ryad,prad,yrad)
+        m2 = self * m2
+        self.a = m2.a
+        self.b = m2.b
+        self.c = m2.c
+
+    def rotate_312(self, ryad, prad, yrad):
+        '''rotate the matrix by a given amount in-place on 3 axes with 312 ordering'''
+        m2 = Matrix3()
+        m2.from_euler312(ryad,prad,yrad)
+        m2 = self * m2
+        self.a = m2.a
+        self.b = m2.b
+        self.c = m2.c
+
     def normalize(self):
         '''re-normalise a rotation matrix'''
         error = self.a * self.b

tests/test_rotmat.py

@@ -97,6 +97,32 @@ def test_euler(self):
                     v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
                     diff = v1 - v2
                     assert diff.length() < 1.0e-12
+                    # construct the rotation using 321 ordering
+                    m2 = Matrix3()
+                    m2.rotate_yaw(radians(y))
+                    m2.rotate_pitch(radians(p))
+                    m2.rotate_roll(radians(r))
+                    (r2, p2, y2) = m2.to_euler()
+                    v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
+                    diff = v1 - v2
+                    assert diff.length() < 1.0e-12
+        # test rotate_321()
+        (r1,p1,y1) = radians(17),radians(-35),radians(126)
+        (r2,p2,y2) = radians(-39),radians(63),radians(-18)
+        m.from_euler(r1,p1,y1)
+        m2 = m.copy()
+        m2.rotate_yaw(y2)
+        m2.rotate_pitch(p2)
+        m2.rotate_roll(r2)
+        (r3,p3,y3) = m2.to_euler()
+        v1 = Vector3(degrees(r3),degrees(p3),degrees(y3))
+        m3 = m.copy()
+        m3.rotate_321(r2,p2,y2)
+        (r3,p3,y3) = m3.to_euler()
+        v2 = Vector3(degrees(r3),degrees(p3),degrees(y3))
+        diff = v1 - v2
+        assert diff.length() < 1.0e-12
+
 
     def test_euler312(self):
         '''check that from_euler312() and to_euler312() are consistent'''
@@ -110,6 +136,32 @@ def test_euler312(self):
                     v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
                     diff = v1 - v2
                     assert diff.length() < 1.0e-12
+                    # construct the rotation using 312 ordering
+                    m2 = Matrix3()
+                    m2.identity()
+                    m2.rotate_yaw(radians(y))
+                    m2.rotate_roll(radians(r))
+                    m2.rotate_pitch(radians(p))
+                    (r2, p2, y2) = m2.to_euler312()
+                    v2 = Vector3(degrees(r2), degrees(p2), degrees(y2))
+                    diff = v1 - v2
+                    assert diff.length() < 1.0e-12
+        # test rotate_312()
+        (r1,p1,y1) = radians(17),radians(-35),radians(126)
+        (r2,p2,y2) = radians(-39),radians(63),radians(-18)
+        m.from_euler312(r1,p1,y1)
+        m2 = m.copy()
+        m2.rotate_yaw(y2)
+        m2.rotate_roll(r2)
+        m2.rotate_pitch(p2)
+        (r3,p3,y3) = m2.to_euler312()
+        v1 = Vector3(degrees(r3),degrees(p3),degrees(y3))
+        m3 = m.copy()
+        m3.rotate_312(r2,p2,y2)
+        (r3,p3,y3) = m3.to_euler312()
+        v2 = Vector3(degrees(r3),degrees(p3),degrees(y3))
+        diff = v1 - v2
+        assert diff.length() < 1.0e-12
 
     def test_matrixops(self):
         m1 = Matrix3(Vector3(1, 0, 0), Vector3(1, 5, 0), Vector3(1, 0, -7))
@@ -163,5 +215,6 @@ def test_plane(self):
         p = line.plane_intersection(plane)
         assert p.close(Vector3(11.11, 11.11, 0.00), tol=1e-2)
 
+
 if __name__ == '__main__':
     unittest.main()