Floyd algorithm matlab implementation