Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 00:31:36 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

support symmetric cost in order to save memory

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2026-04-30 14:27:46 +03:00
parent cce490e6f5
commit e5b4e9c52a
3 changed files with 94 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/dijkstra/DijkstraTest.cpp
+60 -13
View File
@@ -64,7 +64,7 @@ TEST(DijkstraTest, SingleNodeAtGoal)
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 0, cost, best, next, vis));
ASSERT_TRUE(dijkstra::solveBackward(N, 0, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[0], 0.f);
EXPECT_EQ(next[0], -1);
}
@@ -85,7 +85,7 @@ TEST(DijkstraTest, AsymmetricLineGraphForward)
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, best, next, vis));
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, false, 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
@@ -112,7 +112,7 @@ TEST(DijkstraTest, AsymmetricLineGraphReverse)
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 0, cost, best, next, vis));
ASSERT_TRUE(dijkstra::solveBackward(N, 0, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[0], 0.f);
EXPECT_FLOAT_EQ(best[1], 100.f);
@@ -137,13 +137,13 @@ TEST(DijkstraTest, TriangleWithBlockedEdge)
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, best, next, vis));
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, false, 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));
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[0], 2.f);
EXPECT_EQ(next[0], 1);
}
@@ -163,7 +163,7 @@ TEST(DijkstraTest, GoalUnreachableNoInfiniteLoop)
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, best, next, vis));
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[2], 0.f);
EXPECT_FALSE(best[0] < dijkstra::kUnreachable);
EXPECT_FALSE(best[1] < dijkstra::kUnreachable);
@@ -185,7 +185,7 @@ TEST(DijkstraTest, NaNTreatedAsMissingEdge)
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, best, next, vis));
ASSERT_TRUE(dijkstra::solveBackward(N, 2, cost, false, best, next, vis));
EXPECT_FLOAT_EQ(best[0], 2.f);
EXPECT_EQ(next[0], 1);
}
@@ -198,11 +198,58 @@ TEST(DijkstraTest, RejectsInvalidInputs)
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));
EXPECT_FALSE(dijkstra::solveBackward(0, 0, cost, false, best, next, vis));
EXPECT_FALSE(dijkstra::solveBackward(N, -1, cost, false, best, next, vis));
EXPECT_FALSE(dijkstra::solveBackward(N, N, cost, false, best, next, vis));
EXPECT_FALSE(dijkstra::solveBackward(N, 0, nullptr, false, best, next, vis));
EXPECT_FALSE(dijkstra::solveBackward(N, 0, cost, false, nullptr, next, vis));
}
TEST(DijkstraTest, SymmetricPackedMatchesAsymmetric)
{
// Same undirected weighted graph expressed two ways: full N*N matrix with both
// halves filled, and packed upper triangle. Results must match for both layouts.
constexpr int N = 5;
// Edges (a, b, w) with a < b.
struct UndirEdge { int a, b; float w; };
const UndirEdge edges[] = {
{0, 1, 2.f},
{0, 2, 4.f},
{1, 2, 1.f},
{1, 3, 7.f},
{2, 3, 3.f},
{3, 4, 2.f},
};
float full[N * N];
for (int i = 0; i < N * N; ++i) { full[i] = INFINITY; }
constexpr int kPacked = N * (N - 1) / 2;
float packed[kPacked];
for (int i = 0; i < kPacked; ++i) { packed[i] = INFINITY; }
for (const auto &e : edges) {
full[e.a * N + e.b] = e.w;
full[e.b * N + e.a] = e.w;
packed[e.a * (2 * N - e.a - 1) / 2 + (e.b - e.a - 1)] = e.w;
}
float best_full[N], best_pack[N];
int next_full[N], next_pack[N];
bool vis[N];
for (int goal = 0; goal < N; ++goal) {
ASSERT_TRUE(dijkstra::solveBackward(N, goal, full, false, best_full, next_full, vis));
ASSERT_TRUE(dijkstra::solveBackward(N, goal, packed, true, best_pack, next_pack, vis));
for (int i = 0; i < N; ++i) {
EXPECT_FLOAT_EQ(best_full[i], best_pack[i]) << "goal=" << goal << " i=" << i;
EXPECT_EQ(next_full[i], next_pack[i]) << "goal=" << goal << " i=" << i;
}
}
}
TEST(DijkstraTest, ForwardWalkReachesGoal)
@@ -223,7 +270,7 @@ TEST(DijkstraTest, ForwardWalkReachesGoal)
float best[N];
int next[N];
bool vis[N];
ASSERT_TRUE(dijkstra::solveBackward(N, 4, cost, best, next, vis));
ASSERT_TRUE(dijkstra::solveBackward(N, 4, cost, false, best, next, vis));
for (int start = 0; start < N - 1; ++start) {
int u = start;
src/lib/dijkstra/dijkstra.cpp
+14 -2
View File
@@ -36,7 +36,7 @@
namespace dijkstra
{
bool solveBackward(int num_nodes, int goal, const float *cost,
bool solveBackward(int num_nodes, int goal, const float *cost, bool symmetric,
float *best_cost, int *next_node, bool *visited)
{
if (num_nodes <= 0 || goal < 0 || goal >= num_nodes
@@ -69,7 +69,19 @@ bool solveBackward(int num_nodes, int goal, const float *cost,
continue;
}
const float edge = cost[v * num_nodes + u];
// Edge v -> u. For asymmetric layout, look up the (v, u) entry directly.
// For symmetric packed upper-triangular: index by (min(v,u), max(v,u)).
// `symmetric` is loop-invariant; the compiler is expected to unswitch.
float edge;
if (symmetric) {
const int a = v < u ? v : u;
const int b = v < u ? u : v;
edge = cost[a * (2 * num_nodes - a - 1) / 2 + (b - a - 1)];
} else {
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.
src/lib/dijkstra/dijkstra.h
+20 -9
View File
@@ -47,10 +47,19 @@
* 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).
* Two cost-matrix layouts are supported, selected by the `symmetric` flag:
*
* - Asymmetric (default, symmetric = false): full N*N row-major matrix.
* `cost[i * num_nodes + j]` is the cost of the directed edge i -> j.
*
* - Symmetric (symmetric = true): packed upper triangle, no diagonal.
* Buffer length is N*(N-1)/2 floats. The cost of the (undirected) edge
* between i and j (i != j) is at:
* offset(i, j) = a*(2*N - a - 1)/2 + (b - a - 1), where a = min(i,j), b = max(i,j)
* Self-loops are not stored; Dijkstra ignores them anyway.
*
* Entries equal to +INFINITY or NaN are treated as missing edges. Negative
* costs are not supported (Dijkstra assumes non-negative edge weights).
*/
#pragma once
@@ -66,11 +75,13 @@ 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 num_nodes number of nodes.
* @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 cost cost buffer; layout depends on `symmetric` (see above).
* Missing edges are encoded as +INFINITY or NaN.
* @param symmetric if true, `cost` is the packed upper triangle of a symmetric
* matrix (length N*(N-1)/2). If false, `cost` is the full
* N*N row-major matrix and edges may be asymmetric.
* @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
@@ -83,7 +94,7 @@ static constexpr float kUnreachable = INFINITY;
* 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,
bool solveBackward(int num_nodes, int goal, const float *cost, bool symmetric,
float *best_cost, int *next_node, bool *visited);
} // namespace dijkstra