Added 3D plot, and local copy of atan2d function