Pathfinder has been upgraded once again. So far walking and following is
really smooth. Even managed to get follow dancing to work. Just need to
upgrade the algorithm still and get it working for npc following.