From 54fb3c895f039041e84f65cb76c044c03f47571e Mon Sep 17 00:00:00 2001
From: Huashuo Lei <139048865+wpyaoxueros@users.noreply.github.com>
Date: Sun, 18 Oct 2026 03:51:59 +0800
Subject: [PATCH] Normalize the axis in UnitQuaternion.AngVec

---
 spatialmath/quaternion.py         |  3 ++
 tests/test_angvec_nonunit_axis.py | 52 +++++++++++++++++++++++++++++++
 2 files changed, 55 insertions(+)
 create mode 100644 tests/test_angvec_nonunit_axis.py

diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py
index 29d68de6..98bffcaa 100644
--- a/spatialmath/quaternion.py
+++ b/spatialmath/quaternion.py
@@ -1551,6 +1551,9 @@ def AngVec(
         """
         v = smb.getvector(v, 3)
         theta = smb.getunit(theta, unit, vector=False)
+        if theta == 0:
+            return cls()
+        v = smb.unitvec(v)
         return cls(
             s=math.cos(theta / 2), v=math.sin(theta / 2) * v, norm=False, check=False
         )
diff --git a/tests/test_angvec_nonunit_axis.py b/tests/test_angvec_nonunit_axis.py
new file mode 100644
index 00000000..8eb3926b
--- /dev/null
+++ b/tests/test_angvec_nonunit_axis.py
@@ -0,0 +1,52 @@
+"""Angle-axis construction preserves unit-quaternion and zero-angle contracts."""
+
+import math
+import unittest
+
+import numpy as np
+from numpy.testing import assert_allclose
+
+from spatialmath import UnitQuaternion
+from spatialmath.base import angvec2r
+
+
+class TestAngVecNonunitAxis(unittest.TestCase):
+    def test_unit_norm(self):
+        assert_allclose(np.linalg.norm(UnitQuaternion.AngVec(0.5, [1, 2, 3]).vec), 1)
+
+    def test_angle_round_trip(self):
+        theta, axis = UnitQuaternion.AngVec(0.5, [1, 2, 3]).angvec()
+        assert_allclose(theta, 0.5)
+        assert_allclose(axis, np.array([1, 2, 3]) / np.sqrt(14))
+
+    def test_rotation_matrix(self):
+        assert_allclose(
+            UnitQuaternion.AngVec(0.5, [1, 2, 3]).R,
+            angvec2r(0.5, [1, 2, 3]),
+            atol=1e-12,
+        )
+
+    def test_degrees(self):
+        assert_allclose(
+            UnitQuaternion.AngVec(90, [0, 0, 4], unit="deg").R,
+            angvec2r(math.pi / 2, [0, 0, 1]),
+            atol=1e-12,
+        )
+
+    def test_unit_axis_control(self):
+        assert_allclose(
+            UnitQuaternion.AngVec(0.5, [1, 0, 0]).vec,
+            [math.cos(0.25), math.sin(0.25), 0, 0],
+        )
+
+    def test_zero_angle_zero_axis(self):
+        assert_allclose(UnitQuaternion.AngVec(0, [0, 0, 0]).vec, [1, 0, 0, 0])
+
+    def test_zero_degrees_zero_axis(self):
+        assert_allclose(
+            UnitQuaternion.AngVec(0, [0, 0, 0], unit="deg").vec, [1, 0, 0, 0]
+        )
+
+    def test_nonzero_angle_requires_nonzero_axis(self):
+        with self.assertRaises(ValueError):
+            UnitQuaternion.AngVec(0.5, [0, 0, 0])