def slerp(q1, q2, u):
    """SLERP: Spherical linear interpolation between two unit quaternions.
    
    Parameters
    ------------
    q1 : (4, ) ndarray
            Unit quaternion (first element scalar)
    q2 : (4, ) ndarray
            Unit quaternion (first element scalar)
    u : float
            Interpolation factor in range [0,1] where 0 is first quaternion 
            and 1 is second quaternion.
            
    Returns
    -----------
    q : (4,) ndarray
            The interpolated unit quaternion
    """
    q1 = q1.flatten()
    q2 = q2.flatten()
    assert q1.shape == q2.shape
    assert q1.size == 4
    costheta = np.dot(q1, q2)

    if np.isclose(u, 0.):
        return q1
    elif np.isclose(u, 1.):
        return q2
    elif u > 1 or u < 0:
        raise ValueError("u must be in range [0, 1]")

    # Shortest path
    if costheta < 0:
        costheta = -costheta
        q2 = -q2

    # Almost the same, we can return any of them?
    if np.isclose(costheta, 1.0):
        return q1

    theta = np.arccos(costheta)

    f1 = np.sin((1.0 - u)*theta) / np.sin(theta)
    f2 = np.sin(u*theta) / np.sin(theta)
    q = f1*q1 + f2*q2
    q = q / np.sqrt(np.sum(q**2)) # Normalize
    return q
SLERP: Spherical linear interpolation between two unit quaternions.
    
    Parameters
    ------------
    q1 : (4, ) ndarray
            Unit quaternion (first element scalar)
    q2 : (4, ) ndarray
            Unit quaternion (first element scalar)
    u : float
            Interpolation factor in range [0,1] where 0 is first quaternion 
            and 1 is second quaternion.
            
    Returns
    -----------
    q : (4,) ndarray
            The interpolated unit quaternion