quaternion.py 文件源码

def to_angle_axis(self):
        """ Return axis-angle representation """
        q = np.roll(self.q, shift=1)
        halftheta = math.acos(q[0])
        if abs(halftheta) < 1e-12:
            return 0, np.array((0, 0, 1))
        else:
            theta = halftheta * 2
            axis = np.array(q[1:4]) / math.sin(halftheta)
            return theta, axis