Added 3d plotting of the path.