diff --git a/src/lib/dijkstra/DijkstraTest.cpp b/src/lib/dijkstra/DijkstraTest.cpp index 5f26c116a4..a7c3b33ecc 100644 --- a/src/lib/dijkstra/DijkstraTest.cpp +++ b/src/lib/dijkstra/DijkstraTest.cpp @@ -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; diff --git a/src/lib/dijkstra/dijkstra.cpp b/src/lib/dijkstra/dijkstra.cpp index 9fae11143a..ecc16f01ed 100644 --- a/src/lib/dijkstra/dijkstra.cpp +++ b/src/lib/dijkstra/dijkstra.cpp @@ -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. diff --git a/src/lib/dijkstra/dijkstra.h b/src/lib/dijkstra/dijkstra.h index f97995fb90..ffd79f0b36 100644 --- a/src/lib/dijkstra/dijkstra.h +++ b/src/lib/dijkstra/dijkstra.h @@ -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