def rotate(x, y, angle, z=None, assert_shape=True):
    """
    Rotate the coordinates (x, y) by the given angle in radians.
    """
    if assert_shape:
        assert angle.shape == x.shape == y.shape, (angle.shape, x.shape, y.shape)
        if z is not None:
            assert x.shape == z.shape
    other_x_trans = np.cos(angle) * x - np.sin(angle) * y
    other_y_trans = np.cos(angle) * y + np.sin(angle) * x
    if z is None:
        output_coords = np.stack((other_x_trans, other_y_trans), axis=-1)
    else:
        output_coords = np.stack((other_x_trans, other_y_trans, z), axis=-1)
    return output_coords

    Rotate the coordinates (x, y) by the given angle in radians.
    
metadrive/utils/waypoint_utils.py