From 00757d0404b41d36f315d8f7441d4d65f1a006c1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 30 Oct 2023 07:30:46 +1100
Subject: [PATCH] rotmat: added more euler 321 and 312 functions

---
 rotmat.py            | 45 +++++++++++++++++++++++++++++++++++++
 tests/test_rotmat.py | 53 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 98 insertions(+)

diff --git a/rotmat.py b/rotmat.py
index 86bd1edd5..b9ee4a8f8 100755
--- a/rotmat.py
+++ b/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
diff --git a/tests/test_rotmat.py b/tests/test_rotmat.py
index 05a8f3a6f..1dcfef1f0 100755
--- a/tests/test_rotmat.py
+++ 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()