Optimised pathfinding