def quat_to_rotation_matrix(q):
    """Convert unit quaternion to rotation matrix
    
    Parameters
    -------------
    q : (4,) ndarray
            Unit quaternion, scalar as first element

    Returns
    ----------------
    R : (3,3) ndarray
            Rotation matrix
    
    """
    q = q.flatten()
    assert q.size == 4
    assert_almost_equal(np.linalg.norm(q), 1.0, err_msg="Not a unit quaternion!")
    qq = q ** 2
    R = np.array([[qq[0] + qq[1] - qq[2] - qq[3], 2*q[1]*q[2] -
2*q[0]*q[3], 2*q[1]*q[3] + 2*q[0]*q[2]],
                [2*q[1]*q[2] + 2*q[0]*q[3], qq[0] - qq[1] + qq[2] -
qq[3], 2*q[2]*q[3] - 2*q[0]*q[1]],
                [2*q[1]*q[3] - 2*q[0]*q[2], 2*q[2]*q[3] + 2*q[0]*q[1],
qq[0] - qq[1] - qq[2] + qq[3]]])
    return R
def quat_to_rotation_matrix ( q ) : q = q . flatten ( ) assert q . size == 4 assert_almost_equal ( np . linalg . norm ( q ) , 1.0 , err_msg = "Not a unit quaternion!" ) qq = q ** 2 R = np . array ( [ [ qq [ 0 ] + qq [ 1 ] - qq [ 2 ] - qq [ 3 ] , 2 * q [ 1 ] * q [ 2 ] - 2 * q [ 0 ] * q [ 3 ] , 2 * q [ 1 ] * q [ 3 ] + 2 * q [ 0 ] * q [ 2 ] ] , [ 2 * q [ 1 ] * q [ 2 ] + 2 * q [ 0 ] * q [ 3 ] , qq [ 0 ] - qq [ 1 ] + qq [ 2 ] - qq [ 3 ] , 2 * q [ 2 ] * q [ 3 ] - 2 * q [ 0 ] * q [ 1 ] ] , [ 2 * q [ 1 ] * q [ 3 ] - 2 * q [ 0 ] * q [ 2 ] , 2 * q [ 2 ] * q [ 3 ] + 2 * q [ 0 ] * q [ 1 ] , qq [ 0 ] - qq [ 1 ] - qq [ 2 ] + qq [ 3 ] ] ] ) return R
Convert unit quaternion to rotation matrix
    
    Parameters
    -------------
    q : (4,) ndarray
            Unit quaternion, scalar as first element

    Returns
    ----------------
    R : (3,3) ndarray
            Rotation matrix