Updated Heuristic method in PathfindingTread.cs

Old method is intact, but commented out.  See what you think - we may save some time this way and should get the same results.

I also added a constant for the HEURISTIC_MULTIPLIER.