Fix bug with pathfinding when compiling with -O3

This commit fixes issue #164. Thanks to @dalkon for the patch.