Skip to content

Commit 78ded48

Browse files
committed
Tweak angdist code and expand doco
1 parent e30e41c commit 78ded48

1 file changed

Lines changed: 15 additions & 8 deletions

File tree

‎spatialmath/quaternion.py‎

Lines changed: 15 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -2128,7 +2128,7 @@ def angvec(self, unit='rad'):
21282128
# return Quaternion(s=0, v=math.acos(self.s) * base.unitvec(self.v))
21292129

21302130
def angdist(self, other, metric=0):
2131-
"""
2131+
r"""
21322132
Angular distance metric between unit quaternions
21332133
21342134
:param other: second unit quaternion
@@ -2147,11 +2147,11 @@ def angdist(self, other, metric=0):
21472147
====== ===============================================================
21482148
Metric Details
21492149
====== ===============================================================
2150-
0 :math:`1 - | \q_1 \sbullet \q_2 | \in [0, 1]`
2151-
1 :math:`\cos^{-1} | \q_1 \sbullet \q_2 | \in [0, \pi/2]`
2152-
2 :math:`2 \cos^{-1} | \q_1 \sbullet \q_2 | \in [0, \pi]`
2150+
0 :math:`1 - | \q_1 \bullet \q_2 | \in [0, 1]`
2151+
1 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
2152+
2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
21532153
3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]`
2154-
4 :math:`\cos^{-1} \left( 2 (\q_1 \sbullet \q_2)^2 - 1\right) \in [0, 1]`
2154+
4 :math:`\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]`
21552155
====== ===============================================================
21562156
21572157
Example:
@@ -2165,7 +2165,10 @@ def angdist(self, other, metric=0):
21652165
>>> print(q1.angdist(q2))
21662166
21672167
.. note::
2168-
- metrics 2 and 3 are equivalent
2168+
- metrics 1, 2, 4 can throw ValueError "math domain error" due to
2169+
numeric errors which push the argument of ``acos()`` marginally
2170+
outside its domain [0, 1].
2171+
- metrics 2 and 3 are equivalent, but 3 is more robust
21692172
- SMTB-MATLAB uses metric 3 for UnitQuaternion.angle()
21702173
- MATLAB's quaternion.dist() uses metric 2
21712174
"""
@@ -2177,13 +2180,17 @@ def angdist(self, other, metric=0):
21772180
elif metric == 1:
21782181
measure = lambda p, q: math.acos(np.dot(p, q))
21792182
elif metric == 2:
2180-
measure = lambda p, q: 2 * math.acos(np.dot(p, q))
2183+
measure = lambda p, q: math.acos(np.dot(p, q))
21812184
elif metric == 3:
21822185
measure = lambda p, q: 2 * math.atan2(base.norm(p - q), base.norm(p + q))
21832186
elif metric == 4:
21842187
measure = lambda p, q: math.acos(2 * np.dot(p, q) ** 2 - 1)
21852188

2186-
return self.binop(other, measure)
2189+
ad = self.binop(other, measure)
2190+
if len(ad) == 1:
2191+
return ad[0]
2192+
else:
2193+
return ad
21872194

21882195
def SO3(self):
21892196
"""

0 commit comments

Comments
 (0)