Fix for A* pathing

Previously, the A* algorithm was improperly performing the heuristic
necessary to optimize the path between an entity and the destination.
This fixes the issue in the algorithm itself, but does not fix complex
pathing.