fix speed and diagonal pathfinding