@@ -2127,20 +2127,20 @@ def angvec(self, unit='rad'):
21272127 # """
21282128 # return Quaternion(s=0, v=math.acos(self.s) * base.unitvec(self.v))
21292129
2130- def angdist (self , other , metric = 0 ):
2130+ def angdist (self , other , metric = 3 ):
21312131 r"""
21322132 Angular distance metric between unit quaternions
21332133
21342134 :param other: second unit quaternion
21352135 :type other: UnitQuaternion instance
2136- :param metric: metric, default is 0
2136+ :param metric: metric, default is 3
21372137 :type metric: int
21382138 :raises TypeError: if other is not a UnitQuaternion
21392139 :return: angle in radians
21402140 :rtype: float
21412141
2142- ``q1.angle (q2)`` is the geodesic norm, or geodesic distance between two
2143- unit quaternions and has units of angle . We can consider it as the angle between two quaternions.
2142+ ``q1.angdist (q2)`` is the geodesic norm, or geodesic distance between two
2143+ unit quaternions. We can consider it as the angle between two quaternions.
21442144
21452145 Several metrics are supported:
21462146
@@ -2150,10 +2150,13 @@ def angdist(self, other, metric=0):
21502150 0 :math:`1 - | \q_1 \bullet \q_2 | \in [0, 1]`
21512151 1 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
21522152 2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
2153- 3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]`
2153+ 3 :math:`2 \tan^{-1} \| \q_1 \pm \q_2\| / \|\q_1 \mp \q_2\| \in [0, \pi/2]`
21542154 4 :math:`\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]`
21552155 ====== ===============================================================
21562156
2157+ Metric 3 computes the sum and difference of the quaternions and uses
2158+ the largest value in the denominator.
2159+
21572160 Example:
21582161
21592162 .. runblock:: pycon
@@ -2170,19 +2173,27 @@ def angdist(self, other, metric=0):
21702173 outside its domain [0, 1].
21712174 - metrics 2 and 3 are equivalent, but 3 is more robust
21722175 - SMTB-MATLAB uses metric 3 for UnitQuaternion.angle()
2173- - MATLAB's quaternion.dist() uses metric 2
2176+ - MATLAB's quaternion.dist() uses metric 4
21742177 """
21752178 if not isinstance (other , UnitQuaternion ):
21762179 raise TypeError ('bad operand' )
21772180
2181+ def metric3 (p , q ):
2182+ x = base .norm (p - q )
2183+ y = base .norm (p + q )
2184+ if x >= y :
2185+ return 2 * math .atan (y / x )
2186+ else :
2187+ return 2 * math .atan (x / y )
2188+
21782189 if metric == 0 :
2179- measure = lambda p , q : 1 - np .dot (p , q )
2190+ measure = lambda p , q : 1 - abs ( np .dot (p , q ) )
21802191 elif metric == 1 :
2181- measure = lambda p , q : math .acos (np .dot (p , q ))
2192+ measure = lambda p , q : math .acos (abs ( np .dot (p , q ) ))
21822193 elif metric == 2 :
2183- measure = lambda p , q : math .acos (np .dot (p , q ))
2194+ measure = lambda p , q : math .acos (abs ( np .dot (p , q ) ))
21842195 elif metric == 3 :
2185- measure = lambda p , q : 2 * math . atan2 ( base . norm ( p - q ), base . norm ( p + q ))
2196+ measure = metric3
21862197 elif metric == 4 :
21872198 measure = lambda p , q : math .acos (2 * np .dot (p , q ) ** 2 - 1 )
21882199
0 commit comments