Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Avoid: Early exit Dijkstra's path finder if destination is found

Browse Source
This commit is contained in:
Rishabh 2021-09-30 19:06:04 +05:30 committed by Randy Mackay
parent 98c105c4e2
commit 13f3d04300
1 changed files with 6 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_Avoidance/AP_OADijkstra.cpp
View File

@ -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 // move current_node_idx to node with lowest distance
while (find_closest_node_idx(current_node_idx)) { 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 distances to all neighbours of current node
update_visible_node_distances(current_node_idx); update_visible_node_distances(current_node_idx);