|
| 1 | +"""Angle-axis construction preserves unit-quaternion and zero-angle contracts.""" |
| 2 | + |
| 3 | +import math |
| 4 | +import unittest |
| 5 | + |
| 6 | +import numpy as np |
| 7 | +from numpy.testing import assert_allclose |
| 8 | + |
| 9 | +from spatialmath import UnitQuaternion |
| 10 | +from spatialmath.base import angvec2r |
| 11 | + |
| 12 | + |
| 13 | +class TestAngVecNonunitAxis(unittest.TestCase): |
| 14 | + def test_unit_norm(self): |
| 15 | + assert_allclose(np.linalg.norm(UnitQuaternion.AngVec(0.5, [1, 2, 3]).vec), 1) |
| 16 | + |
| 17 | + def test_angle_round_trip(self): |
| 18 | + theta, axis = UnitQuaternion.AngVec(0.5, [1, 2, 3]).angvec() |
| 19 | + assert_allclose(theta, 0.5) |
| 20 | + assert_allclose(axis, np.array([1, 2, 3]) / np.sqrt(14)) |
| 21 | + |
| 22 | + def test_rotation_matrix(self): |
| 23 | + assert_allclose( |
| 24 | + UnitQuaternion.AngVec(0.5, [1, 2, 3]).R, |
| 25 | + angvec2r(0.5, [1, 2, 3]), |
| 26 | + atol=1e-12, |
| 27 | + ) |
| 28 | + |
| 29 | + def test_degrees(self): |
| 30 | + assert_allclose( |
| 31 | + UnitQuaternion.AngVec(90, [0, 0, 4], unit="deg").R, |
| 32 | + angvec2r(math.pi / 2, [0, 0, 1]), |
| 33 | + atol=1e-12, |
| 34 | + ) |
| 35 | + |
| 36 | + def test_unit_axis_control(self): |
| 37 | + assert_allclose( |
| 38 | + UnitQuaternion.AngVec(0.5, [1, 0, 0]).vec, |
| 39 | + [math.cos(0.25), math.sin(0.25), 0, 0], |
| 40 | + ) |
| 41 | + |
| 42 | + def test_zero_angle_zero_axis(self): |
| 43 | + assert_allclose(UnitQuaternion.AngVec(0, [0, 0, 0]).vec, [1, 0, 0, 0]) |
| 44 | + |
| 45 | + def test_zero_degrees_zero_axis(self): |
| 46 | + assert_allclose( |
| 47 | + UnitQuaternion.AngVec(0, [0, 0, 0], unit="deg").vec, [1, 0, 0, 0] |
| 48 | + ) |
| 49 | + |
| 50 | + def test_nonzero_angle_requires_nonzero_axis(self): |
| 51 | + with self.assertRaises(ValueError): |
| 52 | + UnitQuaternion.AngVec(0.5, [0, 0, 0]) |
0 commit comments