From b4e5596ca2917feb7d3ab2bf2457d4e4c656e687 Mon Sep 17 00:00:00 2001 From: Salad Dais Date: Mon, 8 Aug 2022 00:38:09 +0000 Subject: [PATCH] Add more utils for converting between quat and euler --- hippolyzer/lib/base/datatypes.py | 13 +++++++++++++ tests/base/test_datatypes.py | 14 ++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/hippolyzer/lib/base/datatypes.py b/hippolyzer/lib/base/datatypes.py index 022f7f7..d49ed4f 100644 --- a/hippolyzer/lib/base/datatypes.py +++ b/hippolyzer/lib/base/datatypes.py @@ -29,6 +29,7 @@ import math from typing import * import recordclass +import transformations logger = getLogger('hippolyzer.lib.base.datatypes') @@ -220,6 +221,15 @@ class Quaternion(TupleCoord): ) return super().__mul__(other) + @classmethod + def from_transformations(cls, coord) -> Quaternion: + """Convert to W (S) last form""" + return cls(coord[1], coord[2], coord[3], coord[0]) + + def to_transformations(self) -> Tuple[float, float, float, float]: + """Convert to W (S) first form for use with the transformations lib""" + return self.W, self.X, self.Y, self.Z + @classmethod def from_euler(cls, roll, pitch, yaw, degrees=False): if degrees: @@ -241,6 +251,9 @@ class Quaternion(TupleCoord): return cls(X=x, Y=y, Z=z, W=w) + def to_euler(self) -> Vector3: + return Vector3(*transformations.euler_from_quaternion(self.to_transformations())) + def data(self, wanted_components=None): if wanted_components == 3: return self.X, self.Y, self.Z diff --git a/tests/base/test_datatypes.py b/tests/base/test_datatypes.py index e772f65..7ba4b16 100644 --- a/tests/base/test_datatypes.py +++ b/tests/base/test_datatypes.py @@ -79,6 +79,20 @@ class TestDatatypes(unittest.TestCase): quat = Quaternion(X=128.0, Y=128.0, Z=22.0) self.assertEqual(quat, (128.0, 128.0, 22.0, 0.0)) + def test_quaternion_euler_roundtrip(self): + orig_vec = Vector3(0.0, -1.0, 2.0) + quat = Quaternion.from_euler(*orig_vec) + for orig_comp, new_comp in zip(orig_vec, quat.to_euler()): + self.assertAlmostEqual(orig_comp, new_comp) + + def test_quaternion_transformations(self): + quat = Quaternion(0.4034226801113349, -0.2590347239999257, 0.7384602626041288, 0.4741598817790379) + expected_trans = (0.4741598817790379, 0.4034226801113349, -0.2590347239999257, 0.7384602626041288) + trans_quat = quat.to_transformations() + self.assertSequenceEqual(expected_trans, trans_quat) + new_quat = Quaternion.from_transformations(trans_quat) + self.assertEqual(quat, new_quat) + def test_uuid_from_bytes(self): tmp_uuid = uuid.UUID('2b7f7a6e-32c5-dbfd-e2c7-926d1a9f0aca') tmp_uuid2 = uuid.UUID('1dd5efe2-faaf-1864-5ac9-bc61c5d8d7ea')