refactored so that heading = 0 is facing east (seems to be a std. in robotics)