updated pathfinding algorithm with increated performance up to 600 percent