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()