diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 5a706fded1..486de055d3 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -47,6 +47,7 @@ add_subdirectory(conversion EXCLUDE_FROM_ALL) add_subdirectory(crc EXCLUDE_FROM_ALL) add_subdirectory(crypto EXCLUDE_FROM_ALL) add_subdirectory(dataman_client EXCLUDE_FROM_ALL) +add_subdirectory(dijkstra EXCLUDE_FROM_ALL) add_subdirectory(drivers EXCLUDE_FROM_ALL) add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL) add_subdirectory(geo EXCLUDE_FROM_ALL) diff --git a/src/lib/dijkstra/CMakeLists.txt b/src/lib/dijkstra/CMakeLists.txt new file mode 100644 index 0000000000..914ef22509 --- /dev/null +++ b/src/lib/dijkstra/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2026 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(dijkstra + dijkstra.cpp +) + +target_include_directories(dijkstra PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC DijkstraTest.cpp LINKLIBS dijkstra) diff --git a/src/lib/dijkstra/DijkstraTest.cpp b/src/lib/dijkstra/DijkstraTest.cpp new file mode 100644 index 0000000000..5f26c116a4 --- /dev/null +++ b/src/lib/dijkstra/DijkstraTest.cpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include "dijkstra.h" + +namespace +{ + +// Helper: assemble cost matrix entries into a row-major buffer. Use INFINITY for +// missing edges; explicit list of finite (i, j, cost) triples is concise in tests. +struct Edge { int from, to; float cost; }; + +template +void makeMatrix(float (&m)[N * N], std::initializer_list edges) +{ + for (int i = 0; i < N * N; ++i) { + m[i] = INFINITY; + } + + for (const auto &e : edges) { + m[e.from * N + e.to] = e.cost; + } +} + +} // namespace + +TEST(DijkstraTest, SingleNodeAtGoal) +{ + constexpr int N = 1; + float cost[N * N] = { INFINITY }; + float best[N]; + int next[N]; + bool vis[N]; + + ASSERT_TRUE(dijkstra::solveBackward(N, 0, cost, best, next, vis)); + EXPECT_FLOAT_EQ(best[0], 0.f); + EXPECT_EQ(next[0], -1); +} + +TEST(DijkstraTest, AsymmetricLineGraphForward) +{ + // 0 -> 1 -> 2, going forward is cheap, going backward is expensive. + constexpr int N = 3; + float cost[N * N]; + makeMatrix(cost, { + {0, 1, 1.f}, + {1, 2, 1.f}, + {1, 0, 100.f}, + {2, 1, 100.f}, + }); + + float best[N]; + int next[N]; + bool vis[N]; + + ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, best, next, vis)); + + EXPECT_FLOAT_EQ(best[0], 2.f); // 0 -> 1 -> 2 costs 1 + 1 + EXPECT_FLOAT_EQ(best[1], 1.f); // 1 -> 2 + EXPECT_FLOAT_EQ(best[2], 0.f); + EXPECT_EQ(next[0], 1); + EXPECT_EQ(next[1], 2); + EXPECT_EQ(next[2], -1); +} + +TEST(DijkstraTest, AsymmetricLineGraphReverse) +{ + // Same matrix as above; with goal = 0, paths must use the expensive back-edges, + // confirming the solver respects edge direction (uses cost[i][j] = i -> j, not j -> i). + constexpr int N = 3; + float cost[N * N]; + makeMatrix(cost, { + {0, 1, 1.f}, + {1, 2, 1.f}, + {1, 0, 100.f}, + {2, 1, 100.f}, + }); + + float best[N]; + int next[N]; + bool vis[N]; + + ASSERT_TRUE(dijkstra::solveBackward(N, 0, cost, best, next, vis)); + + EXPECT_FLOAT_EQ(best[0], 0.f); + EXPECT_FLOAT_EQ(best[1], 100.f); + EXPECT_FLOAT_EQ(best[2], 200.f); + EXPECT_EQ(next[1], 0); + EXPECT_EQ(next[2], 1); + EXPECT_EQ(next[0], -1); +} + +TEST(DijkstraTest, TriangleWithBlockedEdge) +{ + // Direct edge 0 -> 2 is cheap; remove it and the solver must route via 1. + constexpr int N = 3; + float cost[N * N]; + makeMatrix(cost, { + {0, 1, 1.f}, + {1, 2, 1.f}, + {0, 2, 0.5f}, + }); + + float best[N]; + int next[N]; + bool vis[N]; + + ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, best, next, vis)); + EXPECT_FLOAT_EQ(best[0], 0.5f); + EXPECT_EQ(next[0], 2); + + // Block the direct edge and re-solve; path must detour through 1. + cost[0 * N + 2] = INFINITY; + ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, best, next, vis)); + EXPECT_FLOAT_EQ(best[0], 2.f); + EXPECT_EQ(next[0], 1); +} + +TEST(DijkstraTest, GoalUnreachableNoInfiniteLoop) +{ + // Two disconnected components: {0, 1} and {2}. Goal = 2, so 0 and 1 must end + // up unreachable and the algorithm must not loop. + constexpr int N = 3; + float cost[N * N]; + makeMatrix(cost, { + {0, 1, 1.f}, + {1, 0, 1.f}, + }); + + float best[N]; + int next[N]; + bool vis[N]; + + ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, best, next, vis)); + EXPECT_FLOAT_EQ(best[2], 0.f); + EXPECT_FALSE(best[0] < dijkstra::kUnreachable); + EXPECT_FALSE(best[1] < dijkstra::kUnreachable); + EXPECT_EQ(next[0], -1); + EXPECT_EQ(next[1], -1); +} + +TEST(DijkstraTest, NaNTreatedAsMissingEdge) +{ + constexpr int N = 3; + float cost[N * N]; + makeMatrix(cost, { + {0, 1, 1.f}, + {1, 2, 1.f}, + {0, 2, NAN}, // NaN must be treated like INFINITY, not as a 0 / negative edge + }); + + float best[N]; + int next[N]; + bool vis[N]; + + ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, best, next, vis)); + EXPECT_FLOAT_EQ(best[0], 2.f); + EXPECT_EQ(next[0], 1); +} + +TEST(DijkstraTest, RejectsInvalidInputs) +{ + constexpr int N = 2; + float cost[N * N] = { INFINITY, 1.f, 1.f, INFINITY }; + float best[N]; + int next[N]; + bool vis[N]; + + EXPECT_FALSE(dijkstra::solveBackward(0, 0, cost, best, next, vis)); + EXPECT_FALSE(dijkstra::solveBackward(N, -1, cost, best, next, vis)); + EXPECT_FALSE(dijkstra::solveBackward(N, N, cost, best, next, vis)); + EXPECT_FALSE(dijkstra::solveBackward(N, 0, nullptr, best, next, vis)); + EXPECT_FALSE(dijkstra::solveBackward(N, 0, cost, nullptr, next, vis)); +} + +TEST(DijkstraTest, ForwardWalkReachesGoal) +{ + // 5-node graph: 0 -> 1 -> 2 -> 3 -> 4, plus a long detour 0 -> 4 directly. + // Solving once with goal = 4, then for any start we should reach 4 by following + // next_node without re-solving. + constexpr int N = 5; + float cost[N * N]; + makeMatrix(cost, { + {0, 1, 1.f}, + {1, 2, 1.f}, + {2, 3, 1.f}, + {3, 4, 1.f}, + {0, 4, 100.f}, // direct but expensive + }); + + float best[N]; + int next[N]; + bool vis[N]; + ASSERT_TRUE(dijkstra::solveBackward(N, 4, cost, best, next, vis)); + + for (int start = 0; start < N - 1; ++start) { + int u = start; + int hops = 0; + + while (u != 4 && hops < N) { + ASSERT_GE(next[u], 0) << "stuck at " << u << " from start " << start; + u = next[u]; + ++hops; + } + + EXPECT_EQ(u, 4) << "did not reach goal from " << start; + } +} diff --git a/src/lib/dijkstra/dijkstra.cpp b/src/lib/dijkstra/dijkstra.cpp new file mode 100644 index 0000000000..1635b729a9 --- /dev/null +++ b/src/lib/dijkstra/dijkstra.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "dijkstra.h" + +namespace dijkstra +{ + +bool solveBackward(int num_nodes, int goal, const float *cost, + float *best_cost, int *next_node, bool *visited) +{ + if (num_nodes <= 0 || goal < 0 || goal >= num_nodes + || cost == nullptr || best_cost == nullptr || next_node == nullptr || visited == nullptr) { + return false; + } + + for (int i = 0; i < num_nodes; ++i) { + best_cost[i] = kUnreachable; + next_node[i] = -1; + visited[i] = false; + } + + best_cost[goal] = 0.f; + + // Standard O(N^2) Dijkstra on the reverse graph: pick the unvisited node u with + // the smallest current best_cost, then relax every incoming edge v -> u using + // cost(v, u). This yields, for each v, the shortest cost from v to goal and the + // next hop next_node[v] = u along that path. + for (int iter = 0; iter < num_nodes; ++iter) { + int u = -1; + float min_cost = kUnreachable; + + for (int i = 0; i < num_nodes; ++i) { + if (!visited[i] && best_cost[i] < min_cost) { + min_cost = best_cost[i]; + 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) { + if (visited[v]) { + continue; + } + + const float edge = cost[v * num_nodes + u]; + + // Treat +INFINITY and NaN as missing edges. `edge < kUnreachable` is false for + // both because NaN is unordered and INFINITY < INFINITY is false. + if (!(edge < kUnreachable)) { + continue; + } + + const float candidate = u_cost + edge; + + if (candidate < best_cost[v]) { + best_cost[v] = candidate; + next_node[v] = u; + } + } + } + + return true; +} + +} // namespace dijkstra diff --git a/src/lib/dijkstra/dijkstra.h b/src/lib/dijkstra/dijkstra.h new file mode 100644 index 0000000000..f97995fb90 --- /dev/null +++ b/src/lib/dijkstra/dijkstra.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file dijkstra.h + * + * Backward single-source Dijkstra over a precomputed asymmetric cost matrix. + * + * `solveBackward` computes the shortest path from every node to a fixed `goal` + * and returns, for each node `i`, the next node to step toward on its shortest + * path to the goal. The intended usage is: + * + * 1. Whenever the graph (cost matrix) or the goal changes, call solveBackward + * once to fill the next-hop table. + * 2. As long as both stay fixed, querying the path from any new start `s` is + * a constant-time table lookup followed by a forward walk: + * u = s; while (u != goal) { emit(u); u = next_node[u]; } + * No re-planning, no backtracking, no temporary buffers. + * + * The cost matrix is row-major with `cost[i * num_nodes + j]` giving the cost + * of the directed edge i -> j. Costs may be asymmetric. Entries equal to + * +INFINITY or NaN are treated as missing edges. Negative costs are not + * supported (Dijkstra assumes non-negative edge weights). + */ + +#pragma once + +#include + +namespace dijkstra +{ + +/** Sentinel cost for unreachable nodes / missing edges. */ +static constexpr float kUnreachable = INFINITY; + +/** + * Compute backward shortest paths from every node to `goal`. + * + * @param num_nodes number of nodes; cost is num_nodes x num_nodes row-major. + * @param goal target node index in [0, num_nodes). + * @param cost row-major N*N matrix; cost[i*num_nodes + j] is the cost of + * the directed edge i -> j, or +INFINITY / NaN if there is + * no edge. The diagonal is ignored. + * @param best_cost out, length num_nodes: best_cost[i] = shortest cost from i + * to goal, or kUnreachable. best_cost[goal] = 0. + * @param next_node out, length num_nodes: next_node[i] = the node to step to + * from i on the shortest path to goal, or -1 if i == goal or + * i has no path to goal. + * @param visited scratch buffer, length num_nodes; contents on return are + * not meaningful. + * + * @return true if inputs are valid (non-null, num_nodes > 0, goal in range); + * false otherwise. A `true` return does not imply that goal is + * reachable from any particular node — check best_cost[i] for that. + */ +bool solveBackward(int num_nodes, int goal, const float *cost, + float *best_cost, int *next_node, bool *visited); + +} // namespace dijkstra