Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

make search even more efficient by combining two loops

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2026-04-30 14:03:07 +03:00
parent a3cec7a51d
commit 12b3acd5b4
1 changed files with 30 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/dijkstra/dijkstra.cpp
+27 -26
View File
@@ -51,30 +51,18 @@ bool solveBackward(int num_nodes, int goal, const float *cost,
} }
best_cost[goal] = 0.f; best_cost[goal] = 0.f;
visited[goal] = true;
// Standard O(N^2) Dijkstra on the reverse graph: pick the unvisited node u with // O(N^2) Dijkstra on the reverse graph. Each iteration relaxes incoming edges
// the smallest current best_cost, then relax every incoming edge v -> u using // v -> u using cost(v, u) and, in the same sweep over v, tracks the smallest
// cost(v, u). This yields, for each v, the shortest cost from v to goal and the // best_cost among still-unvisited nodes — that argmin is the u for the next
// next hop next_node[v] = u along that path. // iteration. The very first u is `goal` (the only node with best_cost == 0).
for (int iter = 0; iter < num_nodes; ++iter) { int u = goal;
int u = -1; float u_cost = 0.f;
float min_cost = kUnreachable;
for (int i = 0; i < num_nodes; ++i) { for (int iter = 0; iter < num_nodes - 1; ++iter) {
if (!visited[i] && best_cost[i] < min_cost) { int next_u = -1;
min_cost = best_cost[i]; float next_cost = kUnreachable;
u = i;
}
}
if (u < 0) {
// No unvisited node is reachable; remaining nodes stay at kUnreachable.
break;
}
visited[u] = true;
const float u_cost = best_cost[u];
for (int v = 0; v < num_nodes; ++v) { for (int v = 0; v < num_nodes; ++v) {
if (visited[v]) { if (visited[v]) {
@@ -85,10 +73,7 @@ bool solveBackward(int num_nodes, int goal, const float *cost,
// Treat +INFINITY and NaN as missing edges. `edge < kUnreachable` is false for // Treat +INFINITY and NaN as missing edges. `edge < kUnreachable` is false for
// both because NaN is unordered and INFINITY < INFINITY is false. // both because NaN is unordered and INFINITY < INFINITY is false.
if (!(edge < kUnreachable)) { if (edge < kUnreachable) {
continue;
}
const float candidate = u_cost + edge; const float candidate = u_cost + edge;
if (candidate < best_cost[v]) { if (candidate < best_cost[v]) {
@@ -96,6 +81,22 @@ bool solveBackward(int num_nodes, int goal, const float *cost,
next_node[v] = u; next_node[v] = u;
} }
} }
// Same sweep: track argmin of best_cost over unvisited nodes for the next iter.
if (best_cost[v] < next_cost) {
next_cost = best_cost[v];
next_u = v;
}
}
if (next_u < 0) {
// No unvisited node is reachable; remaining nodes stay at kUnreachable.
break;
}
visited[next_u] = true;
u = next_u;
u_cost = next_cost;
} }
return true; return true;