From 13f3d04300a932cd76f14b594a75fe0486bcf364 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Thu, 30 Sep 2021 19:06:04 +0530 Subject: [PATCH] AC_Avoid: Early exit Dijkstra's path finder if destination is found --- libraries/AC_Avoidance/AP_OADijkstra.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index ec8388357f..3dac088bdc 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/libraries/AC_Avoidance/AP_OADijkstra.cpp @@ -830,6 +830,12 @@ bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &d // move current_node_idx to node with lowest distance while (find_closest_node_idx(current_node_idx)) { + node_index dest_node; + // See if this next "closest" node is actually the destination + if (find_node_from_id({AP_OAVisGraph::OATYPE_DESTINATION,0}, dest_node) && current_node_idx == dest_node) { + // We have discovered destination.. Don't bother with the rest of the graph + break; + } // update distances to all neighbours of current node update_visible_node_distances(current_node_idx);