Fixed potential crash in path smoothing.
Fixed A* clearances calculation.