Added path-finding algorithm for units