Added option to save reachable positions

Added an optional parameter to both 2D and 3D plotting to save_to_file. If set, the calculated reachable coordinates will be saved to a .mat file