Improved pathfinding and footprints