Hoist loop-invariant slerp setup

Prepare the validated quaternion pair and stable angle terms once per interpolation batch, then reuse them for each sample. This removes the scale-dependent regression introduced when interp() and interp1() were routed through qslerp(), while keeping the public scalar qslerp behavior byte-equivalent.

Author: binggao1230

spatialmath/base/quaternions.py

@@ -17,7 +17,7 @@
 from spatialmath.base.argcheck import getunit
 from spatialmath.base.types import *
 import scipy.interpolate as interpolate
-from typing import Optional
+from typing import Callable, Optional
 from functools import lru_cache
 import warnings
 
@@ -771,6 +771,53 @@ def r2q(
 #         return np.r_[qs, (math.sqrt(1.0 - qs**2) / nm) * kv]
 
 
+def _qslerp(
+    q0: ArrayLike4,
+    q1: ArrayLike4,
+    shortest: Optional[bool] = False,
+    tol: float = 20,
+) -> Callable[[float], UnitQuaternionArray]:
+    """Prepare an interpolator for a pair of unit quaternions."""
+    q0 = smb.getvector(q0, 4)
+    q1 = smb.getvector(q1, 4)
+    q0_endpoint = q0
+
+    dotprod = np.dot(q0, q1)
+
+    # If the dot product is negative, the quaternions
+    # have opposite handed-ness and slerp won't take
+    # the shorter path. Fix by reversing one quaternion.
+    if shortest:
+        if dotprod < 0:
+            q0 = -q0  # pylint: disable=invalid-unary-operand-type
+            dotprod = -dotprod  # pylint: disable=invalid-unary-operand-type
+
+    dotprod = np.clip(dotprod, -1, 1)
+
+    # sin(theta) is the length of the component of q1 orthogonal to q0.  Computing
+    # it this way keeps full relative precision as theta approaches 0 or pi, where
+    # sin(acos(dotprod)) does not: acos loses the small angle to rounding.
+    sin_theta = float(np.linalg.norm(q1 - dotprod * q0))
+    theta = math.atan2(sin_theta, dotprod)  # theta is the angle between q0 and q1
+
+    def interpolate(s: float) -> UnitQuaternionArray:
+        if s == 0:
+            return q0_endpoint
+        elif s == 1:
+            return q1
+
+        if sin_theta > tol * _eps:
+            s0 = math.sin((1 - s) * theta)
+            s1 = math.sin(s * theta)
+            return ((q0 * s0) + (q1 * s1)) / sin_theta
+        else:
+            # theta is 0 or pi: q0 and q1 are the same rotation, so is every
+            # interpolate between them
+            return q0
+
+    return interpolate
+
+
 def qslerp(
     q0: ArrayLike4,
     q1: ArrayLike4,
@@ -822,40 +869,7 @@ def qslerp(
     """
     if not 0 <= s <= 1:
         raise ValueError("s must be in the interval [0,1]")
-    q0 = smb.getvector(q0, 4)
-    q1 = smb.getvector(q1, 4)
-
-    if s == 0:
-        return q0
-    elif s == 1:
-        return q1
-
-    dotprod = np.dot(q0, q1)
-
-    # If the dot product is negative, the quaternions
-    # have opposite handed-ness and slerp won't take
-    # the shorter path. Fix by reversing one quaternion.
-    if shortest:
-        if dotprod < 0:
-            q0 = -q0  # pylint: disable=invalid-unary-operand-type
-            dotprod = -dotprod  # pylint: disable=invalid-unary-operand-type
-
-    dotprod = np.clip(dotprod, -1, 1)  # Clip within domain of acos()
-
-    # sin(theta) is the length of the component of q1 orthogonal to q0.  Computing
-    # it this way keeps full relative precision as theta approaches 0 or pi, where
-    # sin(acos(dotprod)) does not: acos loses the small angle to rounding.
-    sin_theta = float(np.linalg.norm(q1 - dotprod * q0))
-    theta = math.atan2(sin_theta, dotprod)  # theta is the angle between q0 and q1
-
-    if sin_theta > tol * _eps:
-        s0 = math.sin((1 - s) * theta)
-        s1 = math.sin(s * theta)
-        return ((q0 * s0) + (q1 * s1)) / sin_theta
-    else:
-        # theta is 0 or pi: q0 and q1 are the same rotation, so is every
-        # interpolate between them
-        return q0
+    return _qslerp(q0, q1, shortest=shortest, tol=tol)(s)
 
 
 def _compute_cdf_sin_squared(theta: float):

spatialmath/quaternion.py

@@ -19,6 +19,7 @@
 import numpy as np
 from typing import Any
 import spatialmath.base as smb
+from spatialmath.base.quaternions import _qslerp
 from spatialmath.pose3d import SO3, SE3
 from spatialmath.baseposelist import BasePoseList
 from spatialmath.base.types import *
@@ -1950,9 +1951,8 @@ def interp(
         if not isinstance(end, UnitQuaternion):
             raise TypeError("end argument must be a UnitQuaternion")
 
-        return UnitQuaternion(
-            [smb.qslerp(self.vec, end.vec, sk, shortest=shortest) for sk in s]
-        )
+        interpolate = _qslerp(self.vec, end.vec, shortest=shortest)
+        return UnitQuaternion([interpolate(sk) for sk in s])
 
     def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuaternion:
         """
@@ -1999,9 +1999,8 @@ def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuatern
             s = smb.getvector(s)
             s = np.clip(s, 0, 1)  # enforce valid values
 
-        return UnitQuaternion(
-            [smb.qslerp(smb.qeye(), self.vec, sk, shortest=shortest) for sk in s]
-        )
+        interpolate = _qslerp(smb.qeye(), self.vec, shortest=shortest)
+        return UnitQuaternion([interpolate(sk) for sk in s])
 
     def increment(self, w: ArrayLike3, normalize: Optional[bool] = False) -> None:
         """

tests/test_quaternion.py

@@ -2,6 +2,7 @@
 from math import pi
 import numpy.testing as nt
 import unittest
+from unittest.mock import patch
 
 from spatialmath import *
 from spatialmath.base import *
@@ -732,6 +733,15 @@ def test_interp_same_rotation(self):
         for qi in p.interp(m, 5):
             nt.assert_array_almost_equal(qi.R, p.R)
 
+    def test_interp_prepares_slerp_once(self):
+        q0 = UnitQuaternion.RPY([0.2, 0.3, 0.4])
+        q1 = UnitQuaternion.RPY([-0.3, 0.1, 0.2])
+
+        for interpolate in (lambda: q0.interp1(5), lambda: q0.interp(q1, 5)):
+            with patch("spatialmath.base.quaternions.np.dot", wraps=np.dot) as dot:
+                self.assertEqual(len(interpolate()), 5)
+            self.assertEqual(dot.call_count, 1)
+
     def test_increment(self):
         q = UnitQuaternion()