rotate.py 文件源码

def calc_fac_dfac(q0):
    """
    Calculate the prefactor mapping the quaternion to the exponential map
    and also its derivative. Takes the first element of the quaternion only
    """
    # Ill-defined around q0=1.0
    qm1 = q0-1.0
    # if np.abs(q0) == 1.0:
    #     fac = 2
    #     dfac = -2/3
    if np.abs(qm1) < 1e-8:
        fac = 2 - 2*qm1/3
        dfac = -2/3
    else:
        fac = 2*np.arccos(q0)/np.sqrt(1-q0**2)
        dfac = -2/(1-q0**2)
        dfac += 2*q0*np.arccos(q0)/(1-q0**2)**1.5
    return fac, dfac