@@ -2018,34 +2018,63 @@ def angvec(self, unit='rad'):
20182018 # """
20192019 # return Quaternion(s=0, v=math.acos(self.s) * base.unitvec(self.v))
20202020
2021- def angle (self , other ):
2021+ def angdist (self , other , metric = 0 ):
20222022 """
2023- Angle between unit quaternions
2023+ Angular distance metric between unit quaternions
20242024
20252025 :param other: second unit quaternion
20262026 :type other: UnitQuaternion instance
2027+ :param metric: metric, default is 0
2028+ :type metric: int
20272029 :raises TypeError: if other is not a UnitQuaternion
20282030 :return: angle in radians
20292031 :rtype: float
20302032
20312033 ``q1.angle(q2)`` is the geodesic norm, or geodesic distance between two
20322034 unit quaternions and has units of angle. We can consider it as the angle between two quaternions.
20332035
2036+ Several metrics are supported:
2037+
2038+ ====== ===============================================================
2039+ Metric Details
2040+ ====== ===============================================================
2041+ 0 :math:`1 - | \q_1 \sbullet \q_2 | \in [0, 1]`
2042+ 1 :math:`\cos^{-1} | \q_1 \sbullet \q_2 | \in [0, \pi/2]`
2043+ 2 :math:`2 \cos^{-1} | \q_1 \sbullet \q_2 | \in [0, \pi]`
2044+ 3 :math:`2 \t an^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]`
2045+ 4 :math:`\cos^{-1} \left( 2 (\q_1 \sbullet \q_2)^2 - 1\r ight) \in [0, 1]`
2046+ ====== ===============================================================
2047+
20342048 Example:
20352049
20362050 .. runblock:: pycon
20372051
20382052 >>> from spatialmath import UnitQuaternion
20392053 >>> q1 = UnitQuaternion.Rx(0.3)
2040- >>> q2 = UnitQuaternion.Rx (0.3)
2041- >>> print(q1.angle (q1))
2042- >>> print(q1.angle (q2))
2054+ >>> q2 = UnitQuaternion.Ry (0.3)
2055+ >>> print(q1.angdist (q1))
2056+ >>> print(q1.angdist (q2))
20432057
2058+ .. note::
2059+ - metrics 2 and 3 are equivalent
2060+ - SMTB-MATLAB uses metric 3 for UnitQuaternion.angle()
2061+ - MATLAB's quaternion.dist() uses metric 2
20442062 """
20452063 if not isinstance (other , UnitQuaternion ):
20462064 raise TypeError ('bad operand' )
20472065
2048- return self .binop (other , lambda p , q : math .acos (2 * np .dot (p , q ) ** 2 - 1 ))
2066+ if metric == 0 :
2067+ measure = lambda p , q : 1 - np .dot (p , q )
2068+ elif metric == 1 :
2069+ measure = lambda p , q : math .acos (np .dot (p , q ))
2070+ elif metric == 2 :
2071+ measure = lambda p , q : 2 * math .acos (np .dot (p , q ))
2072+ elif metric == 3 :
2073+ measure = lambda p , q : 2 * math .atan2 (base .norm (p - q ), base .norm (p + q ))
2074+ elif metric == 4 :
2075+ measure = lambda p , q : math .acos (2 * np .dot (p , q ) ** 2 - 1 )
2076+
2077+ return self .binop (other , measure )
20492078
20502079 def SO3 (self ):
20512080 """
0 commit comments