MADS: Fix pathfinding when dest pos is exactly the same as source pos