Pathnode and PathFind

Added the PathNode and PathFind class.
AI uses correct path most of the time.
Need to find a way to make sure he always "finds" the shortest path.