Modified from https://webdocs.cs.ualberta.ca/~games/pathfind/ HPA* just fix some tiny bugs about #include