Added pathfinding

source:
http://www.redblobgames.com/pathfinding/a-star/implementation.html#cplusplus