Add findPath method based on an A*-algorithm