From 3b6e6e7e34c19d120509b58f93d8daf89d076775 Mon Sep 17 00:00:00 2001
From: Jien Cao <jcao@theaiinstitute.com>
Date: Sun, 5 May 2024 12:21:54 -0400
Subject: [PATCH] add tests

---
 spatialmath/base/quaternions.py  |  2 +-
 spatialmath/base/transforms2d.py |  4 ++++
 spatialmath/baseposematrix.py    |  2 +-
 tests/base/test_quaternions.py   |  7 +++++++
 tests/test_pose2d.py             | 10 ++++++++++
 5 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py
index 516112fe..69292c86 100755
--- a/spatialmath/base/quaternions.py
+++ b/spatialmath/base/quaternions.py
@@ -549,7 +549,7 @@ def r2q(
     check: Optional[bool] = False,
     tol: float = 20,
     order: Optional[str] = "sxyz",
-    shortest: bool = True,
+    shortest: bool = False,
 ) -> UnitQuaternionArray:
     """
     Convert SO(3) rotation matrix to unit-quaternion
diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py
index 0d02807f..342e270a 100644
--- a/spatialmath/base/transforms2d.py
+++ b/spatialmath/base/transforms2d.py
@@ -943,6 +943,10 @@ def trinterp2(start, end, s, shortest: bool = True):
 
             th0 = math.atan2(start[1, 0], start[0, 0])
             th1 = math.atan2(end[1, 0], end[0, 0])
+            if shortest:
+                delta = (th1 - th0 + math.pi) % (math.pi * 2) - math.pi
+                delta = delta + math.pi * 2 if delta < -math.pi else delta
+                th1 = th0 + delta
 
             p0 = transl2(start)
             p1 = transl2(end)
diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py
index 7b279b59..1a850600 100644
--- a/spatialmath/baseposematrix.py
+++ b/spatialmath/baseposematrix.py
@@ -434,7 +434,7 @@ def interp(self, end: Optional[bool] = None, s: Union[int, float] = None, shorte
         if self.N == 2:
             # SO(2) or SE(2)
             return self.__class__(
-                [smb.trinterp2(start=self.A, end=end, s=_s) for _s in s]
+                [smb.trinterp2(start=self.A, end=end, s=_s, shortest=shortest) for _s in s]
             )
 
         elif self.N == 3:
diff --git a/tests/base/test_quaternions.py b/tests/base/test_quaternions.py
index 54977c20..c512c6d2 100644
--- a/tests/base/test_quaternions.py
+++ b/tests/base/test_quaternions.py
@@ -131,6 +131,13 @@ def test_rotation(self):
         )
         nt.assert_array_almost_equal(qvmul([0, 1, 0, 0], [0, 0, 1]), np.r_[0, 0, -1])
 
+        large_rotation = math.pi + 0.01
+        q1 = r2q(tr.rotx(large_rotation), shortest=False)
+        q2 = r2q(tr.rotx(large_rotation), shortest=True)
+        self.assertLess(q1[0], 0)
+        self.assertGreater(q2[0], 0)
+        self.assertTrue(qisequal(q1=q1, q2=q2, unitq=True))
+
     def test_slerp(self):
         q1 = np.r_[0, 1, 0, 0]
         q2 = np.r_[0, 0, 1, 0]
diff --git a/tests/test_pose2d.py b/tests/test_pose2d.py
index e45cc919..d6d96813 100755
--- a/tests/test_pose2d.py
+++ b/tests/test_pose2d.py
@@ -456,6 +456,16 @@ def test_interp(self):
         array_compare(I.interp(TT, s=1), TT)
         array_compare(I.interp(TT, s=0.5), SE2(1, -2, 0.3))
 
+        R1 = SO2(math.pi - 0.1)
+        R2 = SO2(-math.pi + 0.2)
+        array_compare(R1.interp(R2, s=0.5, shortest=False), SO2(0.05))
+        array_compare(R1.interp(R2, s=0.5, shortest=True), SO2(-math.pi + 0.05))
+
+        T1 = SE2(0, 0, math.pi - 0.1)
+        T2 = SE2(0, 0, -math.pi + 0.2)
+        array_compare(T1.interp(T2, s=0.5, shortest=False), SE2(0, 0, 0.05))
+        array_compare(T1.interp(T2, s=0.5, shortest=True), SE2(0, 0, -math.pi + 0.05))
+
     def test_miscellany(self):
         TT = SE2(1, 2, 0.3)