# UNDF: UNDF-2026-000000980
--- a/src/Airports/groundnetwork.cxx
+++ b/src/Airports/groundnetwork.cxx
@@ -433,34 +433,38 @@
 FGTaxiRoute FGGroundNetwork::findShortestRoute(FGTaxiNode* start, FGTaxiNode* end, bool fullSearch)
 {
     if (!start || !end) {
         throw sg_exception("Bad arguments to findShortestRoute");
     }
-    //implements Dijkstra's algorithm to find shortest distance route from start to end
-    //taken from http://en.wikipedia.org/wiki/Dijkstra's_algorithm
-    FGTaxiNodeVector unvisited(m_nodes);
+    // Dijkstra with priority queue instead of linear scan of unvisited vector.
+    // Original used FGTaxiNodeVector unvisited + linear min-search + std::remove
+    // giving O(V^2). Priority queue gives O((V+E) log V).
+    using NodeDist = std::pair<double, FGTaxiNode*>;
+    std::priority_queue<NodeDist, std::vector<NodeDist>, std::greater<NodeDist>> pq;
     std::map<FGTaxiNode*, ShortestPathData> searchData;
+    std::set<FGTaxiNode*> visited;

     searchData[start].score = 0.0;
+    pq.push({0.0, start});

-    while (!unvisited.empty()) {
-        // find lowest scored unvisited
-        FGTaxiNodeRef best = unvisited.front();
-        for (auto i : unvisited) {
-            if (searchData[i].score < searchData[best].score) {
-                best = i;
-            }
-        }
+    while (!pq.empty()) {
+        auto [dist, best] = pq.top();
+        pq.pop();

-        // remove 'best' from the unvisited set
-        FGTaxiNodeVectorIterator newend =
-            remove(unvisited.begin(), unvisited.end(), best);
-        unvisited.erase(newend, unvisited.end());
+        if (visited.count(best)) {
+            continue;  // already processed with shorter distance
+        }
+        visited.insert(best);

         if (best == end) { // found route or best not connected
             break;
         }

         for (auto target : findSegmentsFrom(best)) {
             double edgeLength = dist(best->cart(), target->cart());
-            double alt = searchData[best].score + edgeLength + edgePenalty(target);
-            if (alt < searchData[target].score) { // Relax (u,v)
-                searchData[target].score = alt;
-                searchData[target].previousNode = best;
+            double alt = searchData[best].score + edgeLength
+                + edgePenalty(target);
+            if (alt < searchData[target].score) {
+                searchData[target].score = alt;   // Relax (u,v)
+                searchData[target].previousNode = best;
+                if (!visited.count(target)) {
+                    pq.push({alt, target});
+                }
             }
         } // of outgoing arcs/segments from current best node iteration
-    }     // of unvisited nodes remaining
+    }     // of priority queue iteration