Skip to content

Commit 54fb3c8

Browse files
committed
Normalize the axis in UnitQuaternion.AngVec
1 parent 41e0a4d commit 54fb3c8

2 files changed

Lines changed: 55 additions & 0 deletions

File tree

‎spatialmath/quaternion.py‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1551,6 +1551,9 @@ def AngVec(
15511551
"""
15521552
v = smb.getvector(v, 3)
15531553
theta = smb.getunit(theta, unit, vector=False)
1554+
if theta == 0:
1555+
return cls()
1556+
v = smb.unitvec(v)
15541557
return cls(
15551558
s=math.cos(theta / 2), v=math.sin(theta / 2) * v, norm=False, check=False
15561559
)

‎tests/test_angvec_nonunit_axis.py‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
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

Comments
 (0)