From be6114668ad79a2ec6d31206e8c428bacc0abbae Mon Sep 17 00:00:00 2001
From: Alexander Fabisch <alexander.fabisch@dfki.de>
Date: Tue, 29 Oct 2024 14:29:47 +0100
Subject: [PATCH] Fix double cover function of MRP

---
 pytransform3d/rotations/_mrp.py      | 8 +++++++-
 pytransform3d/test/test_rotations.py | 2 ++
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/pytransform3d/rotations/_mrp.py b/pytransform3d/rotations/_mrp.py
index 5a63034e7..6b70c22b9 100644
--- a/pytransform3d/rotations/_mrp.py
+++ b/pytransform3d/rotations/_mrp.py
@@ -60,6 +60,9 @@ def mrp_double(mrp):
     \boldsymbol{\psi}` represent the same rotation and correspond to two
     antipodal unit quaternions [1]_.
 
+    No rotation is a special case, in which no second representation exists.
+    Only the zero vector represents no rotation.
+
     Parameters
     ----------
     mrp : array-like, shape (3,)
@@ -78,7 +81,10 @@ def mrp_double(mrp):
        http://malcolmdshuster.com/Pub_1993h_J_Repsurv_scan.pdf
     """
     mrp = check_mrp(mrp)
-    return mrp / -np.dot(mrp, mrp)
+    norm = np.dot(mrp, mrp)
+    if norm == 0.0:
+        return mrp
+    return mrp / -norm
 
 
 def concatenate_mrp(mrp1, mrp2):
diff --git a/pytransform3d/test/test_rotations.py b/pytransform3d/test/test_rotations.py
index 322555f99..fc75f30bb 100644
--- a/pytransform3d/test/test_rotations.py
+++ b/pytransform3d/test/test_rotations.py
@@ -2437,6 +2437,8 @@ def test_mrp_double():
     pr.assert_quaternion_equal(q, q_double)
     assert not np.allclose(q, q_double)
 
+    assert_array_almost_equal(np.zeros(3), pr.mrp_double(np.zeros(3)))
+
 
 def test_assert_euler_almost_equal():
     pr.assert_euler_equal(