Finished SnapToGrid function