Local path planner: Fixed problems with equal entry and exit headings + refactoring.