Add the end point to path vector when pathfinding fails

Fixes #600