Fixed movement with pathfinding algorithm. fixes #3