Merge PR #256: fix: preserve rotation quadrants in trlog2

Author: taughz

spatialmath/base/transforms2d.py

@@ -549,7 +549,7 @@ def trlog2(
         else:
             st = T[1, 0]
             ct = T[0, 0]
-            theta = math.atan(st / ct)
+            theta = math.atan2(st, ct)
             if abs(theta) < tol * _eps:
                 tr = T[:2, 2].flatten()
             else:
@@ -564,7 +564,7 @@ def trlog2(
 
     elif isrot2(T, check=check, tol=tol):
         # SO(2) rotation matrix
-        theta = math.atan(T[1, 0] / T[0, 0])
+        theta = math.atan2(T[1, 0], T[0, 0])
         if twist:
             return theta
         else:

tests/base/test_transforms2d.py

@@ -12,7 +12,7 @@
 import unittest
 from math import pi
 import math
-from scipy.linalg import logm
+from scipy.linalg import expm, logm
 import pytest
 import sys
 
@@ -88,6 +88,25 @@ def test_trlog2(self):
 
         nt.assert_array_almost_equal(trlog2(T, twist=True), vexa(logm(T)))
 
+    def test_trlog2_quadrants(self):
+        for c, s in (
+            (1, 0),
+            (math.sqrt(3) / 2, 0.5),
+            (-0.5, math.sqrt(3) / 2),
+            (-0.5, -math.sqrt(3) / 2),
+            (-1, 0),
+            (0, 1),
+            (0, -1),
+        ):
+            R = np.array([[c, -s], [s, c]])
+            for T in (R, rt2tr(R, [1, 2])):
+                for twist in (False, True):
+                    with self.subTest(c=c, s=s, shape=T.shape, twist=twist):
+                        S = trlog2(T, twist=twist)
+                        if twist:
+                            S = skew(S) if T.shape == (2, 2) else skewa(S)
+                        nt.assert_allclose(expm(S), T, atol=1e-12, rtol=0)
+
     def test_trexp2(self):
         R = trexp2(skew(0.5))
         nt.assert_array_almost_equal(R, rot2(0.5))