From 6b2854b5ffa2a5b9585f5b77f13d7e283d96d6c2 Mon Sep 17 00:00:00 2001 From: HONGYUN <1312465376@qq.com> Date: Wed, 3 Jun 2026 11:49:41 +0800 Subject: [PATCH] Add Adaptive A* pathfinding with semantic risk cost --- .../datastructures/graphs/AdaptiveAStar.java | 263 ++++++++++++++++++ .../graphs/AdaptiveAStarTest.java | 165 +++++++++++ 2 files changed, 428 insertions(+) create mode 100644 src/main/java/com/thealgorithms/datastructures/graphs/AdaptiveAStar.java create mode 100644 src/test/java/com/thealgorithms/datastructures/graphs/AdaptiveAStarTest.java diff --git a/src/main/java/com/thealgorithms/datastructures/graphs/AdaptiveAStar.java b/src/main/java/com/thealgorithms/datastructures/graphs/AdaptiveAStar.java new file mode 100644 index 000000000000..17a09d7da92c --- /dev/null +++ b/src/main/java/com/thealgorithms/datastructures/graphs/AdaptiveAStar.java @@ -0,0 +1,263 @@ +package com.thealgorithms.datastructures.graphs; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Comparator; +import java.util.List; +import java.util.PriorityQueue; + +/** + * Adaptive A* (A-star) pathfinding algorithm with semantic cost weighting. + * + * This implementation extends the classical A* algorithm by introducing + * a semantic risk cost layer, as proposed in: + * + * + * Hong Yun, "An Adaptive Path Planning Method for Indoor and Outdoor + * Integrated Navigation," 2025 IEEE International Conference on Machine + * Learning and Intelligent Systems Engineering (MLISE 2025). + * + * + * Cost Function + * f(n) = g(n) + h(n) + lambda * R_sem(n) + * + * + * + * g(n) — actual cost from the start node to node n + * h(n) — heuristic estimate from node n to the goal + * lambda — global semantic weight multiplier + * R_sem(n) — per-node semantic risk value + * (e.g., 2.0 for construction zones, 0.5 for sidewalks, 0.0 for normal) + * + * + * The semantic cost enables the algorithm to prefer safer or more convenient + * routes in indoor/outdoor navigation scenarios, such as avoiding construction + * areas, preferring well-lit paths at night, or prioritizing barrier-free routes. + * + * When all semantic risk values are zero and lambda is zero, the algorithm + * behaves identically to classical A*. + * + * Time Complexity: O((V + E) log V) where V is the number of vertices + * and E is the number of edges. In the worst case, this reduces to O(E) when + * the heuristic provides perfect guidance. + * + * @see + * Classical AStar (without semantic cost) + */ +public final class AdaptiveAStar { + + private AdaptiveAStar() { + } + + /** + * Directed or undirected edge in the graph. + */ + public static class Edge { + private final int from; + private final int to; + private final int weight; + + public Edge(int from, int to, int weight) { + this.from = from; + this.to = to; + this.weight = weight; + } + + public int getFrom() { + return from; + } + + public int getTo() { + return to; + } + + public int getWeight() { + return weight; + } + } + + /** + * Graph represented as an adjacency list. + */ + public static class Graph { + private final ArrayList> adjacencyList; + + public Graph(int nodeCount) { + this.adjacencyList = new ArrayList<>(nodeCount); + for (int i = 0; i < nodeCount; i++) { + this.adjacencyList.add(new ArrayList<>()); + } + } + + /** + * Adds a bidirectional (undirected) edge. + */ + public void addBidirectionalEdge(int from, int to, int weight) { + adjacencyList.get(from).add(new Edge(from, to, weight)); + adjacencyList.get(to).add(new Edge(to, from, weight)); + } + + /** + * Adds a directed edge. + */ + public void addDirectedEdge(int from, int to, int weight) { + adjacencyList.get(from).add(new Edge(from, to, weight)); + } + + public int nodeCount() { + return adjacencyList.size(); + } + + public ArrayList getNeighbors(int node) { + return adjacencyList.get(node); + } + } + + /** + * Holds the result of a pathfinding operation. + */ + public static class PathResult { + private final int totalCost; + private final List path; + private final boolean found; + + public PathResult(int totalCost, List path, boolean found) { + this.totalCost = totalCost; + this.path = path; + this.found = found; + } + + public int getTotalCost() { + return totalCost; + } + + public List getPath() { + return path; + } + + public boolean isFound() { + return found; + } + } + + /** + * Internal node wrapper used in the priority queue. + */ + private static class NodeState { + final int node; + final int gCost; // actual cost from start + final int fCost; // f(n) = g(n) + h(n) + lambda * R_sem(n) + + NodeState(int node, int gCost, int fCost) { + this.node = node; + this.gCost = gCost; + this.fCost = fCost; + } + } + + /** + * Runs the Adaptive A* algorithm. + * + * @param start the starting node index + * @param goal the target node index + * @param graph the graph (adjacency list) + * @param heuristic heuristic values h[n] for each node (e.g., Euclidean distance to goal) + * @param semanticRisk per-node semantic risk values (e.g., 0.0 = normal, 2.0 = construction zone) + * @param lambda global semantic weight multiplier + * @return a {@link PathResult} containing the total cost and path if found + */ + public static PathResult findPath(int start, int goal, Graph graph, + int[] heuristic, double[] semanticRisk, + double lambda) { + int nodeCount = graph.nodeCount(); + if (start < 0 || start >= nodeCount || goal < 0 || goal >= nodeCount) { + return new PathResult(-1, null, false); + } + + // gCost[i] = actual cost from start to node i + int[] gCost = new int[nodeCount]; + Arrays.fill(gCost, Integer.MAX_VALUE); + gCost[start] = 0; + + // parent[i] = predecessor of node i on the best path + int[] parent = new int[nodeCount]; + Arrays.fill(parent, -1); + + // closed[i] = true if node i has been fully explored + boolean[] closed = new boolean[nodeCount]; + + // Priority queue orders by fCost = gCost + heuristic + semantic penalty + PriorityQueue openSet = new PriorityQueue<>( + Comparator.comparingInt(ns -> ns.fCost)); + + int initialFCost = computeFCost(0, heuristic[start], + semanticRisk[start], lambda); + openSet.add(new NodeState(start, 0, initialFCost)); + + while (!openSet.isEmpty()) { + NodeState current = openSet.poll(); + + // If the current node is the goal, reconstruct and return the path + if (current.node == goal) { + List path = reconstructPath(parent, goal); + return new PathResult(current.gCost, path, true); + } + + if (closed[current.node]) { + continue; + } + closed[current.node] = true; + + // Expand neighbors + for (Edge edge : graph.getNeighbors(current.node)) { + int neighbor = edge.getTo(); + + if (closed[neighbor]) { + continue; + } + + int tentativeGCost = current.gCost + edge.getWeight(); + + if (tentativeGCost < gCost[neighbor]) { + gCost[neighbor] = tentativeGCost; + parent[neighbor] = current.node; + + int fCost = computeFCost(tentativeGCost, heuristic[neighbor], + semanticRisk[neighbor], lambda); + openSet.add(new NodeState(neighbor, tentativeGCost, fCost)); + } + } + } + + return new PathResult(-1, null, false); + } + + /** + * Computes the adaptive cost function: + * 
f(n) = g(n) + h(n) + lambda * R_sem(n)
+ * + * @param gCost actual cost from start to current node + * @param heuristic heuristic estimate to goal + * @param semanticRisk per-node semantic risk + * @param lambda semantic weight multiplier + * @return the total f-cost + */ + private static int computeFCost(int gCost, int heuristic, + double semanticRisk, double lambda) { + int semanticPenalty = (int) Math.round(lambda * semanticRisk); + return gCost + heuristic + semanticPenalty; + } + + /** + * Reconstructs the path from start to goal using the parent array. + */ + private static List reconstructPath(int[] parent, int goal) { + List path = new ArrayList<>(); + int current = goal; + while (current != -1) { + path.add(0, current); + current = parent[current]; + } + return path; + } +} diff --git a/src/test/java/com/thealgorithms/datastructures/graphs/AdaptiveAStarTest.java b/src/test/java/com/thealgorithms/datastructures/graphs/AdaptiveAStarTest.java new file mode 100644 index 000000000000..1136de39b949 --- /dev/null +++ b/src/test/java/com/thealgorithms/datastructures/graphs/AdaptiveAStarTest.java @@ -0,0 +1,165 @@ +package com.thealgorithms.datastructures.graphs; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.List; +import org.junit.jupiter.api.Test; + +public class AdaptiveAStarTest { + + /** + * Builds a simple grid-like graph used for indoor/outdoor navigation testing. + * + * 0 --1-- 1 --1-- 2 + * | | | + * 1 3 1 + * | | | + * 3 --1-- 4 --1-- 5 + * + * Semantic risk defined as: + * - Node 1: construction zone (risk = 2.0) + * - Node 4: sidewalk (risk = 0.5) + * - Others: normal (risk = 0.0) + */ + private AdaptiveAStar.Graph buildTestGraph() { + AdaptiveAStar.Graph graph = new AdaptiveAStar.Graph(6); + graph.addBidirectionalEdge(0, 1, 1); + graph.addBidirectionalEdge(0, 3, 1); + graph.addBidirectionalEdge(1, 2, 1); + graph.addBidirectionalEdge(1, 4, 3); + graph.addBidirectionalEdge(3, 4, 1); + graph.addBidirectionalEdge(4, 5, 1); + graph.addBidirectionalEdge(2, 5, 1); + return graph; + } + + @Test + public void testClassicalAStarEquivalence() { + // When lambda = 0, Adaptive A* should behave identically to classical A* + AdaptiveAStar.Graph graph = buildTestGraph(); + int[] heuristic = {3, 2, 1, 2, 1, 0}; // Manhattan distance to node 5 + double[] semanticRisk = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + AdaptiveAStar.PathResult result = AdaptiveAStar.findPath(0, 5, graph, heuristic, semanticRisk, 0.0); + + assertTrue(result.isFound()); + assertEquals(3, result.getTotalCost()); // shortest path cost: 0→3→4→5 (1+1+1) or 0→1→2→5 (1+1+1) + } + + @Test + public void testAvoidsHighRiskConstructionZone() { + // Node 1 has high risk (construction zone, R_sem=2.0) + // With lambda=2.0, the algorithm should avoid node 1 + // Path via node 1 (0→1→2→5): cost=3, heuristic path=2+2+1+0 + // f(1) = 1 + 2 + 2*2 = 7 + // Path via node 3 (0→3→4→5): cost=3, no risk + // f(3) = 1 + 2 + 2*0 = 3 + AdaptiveAStar.Graph graph = buildTestGraph(); + int[] heuristic = {3, 2, 1, 2, 1, 0}; + double[] semanticRisk = {0.0, 2.0, 0.0, 0.0, 0.5, 0.0}; + + AdaptiveAStar.PathResult result = AdaptiveAStar.findPath(0, 5, graph, heuristic, semanticRisk, 2.0); + + assertTrue(result.isFound()); + assertEquals(3, result.getTotalCost()); + // Should prefer safer route via nodes 3→4→5 + assertEquals(List.of(0, 3, 4, 5), result.getPath()); + } + + @Test + public void testPrefersLowRiskSidewalk() { + // When construction risk is high, prefer sidewalk (lower semantic risk) + // Path 0→1→2→5 has risk at node 1 (2.0) + // Path 0→3→4→5 has mild risk at node 4 (0.5) + AdaptiveAStar.Graph graph = buildTestGraph(); + int[] heuristic = {3, 2, 1, 2, 1, 0}; + double[] semanticRisk = {0.0, 2.0, 0.0, 0.0, 0.5, 0.0}; + + AdaptiveAStar.PathResult result = AdaptiveAStar.findPath(0, 5, graph, heuristic, semanticRisk, 1.0); + + assertTrue(result.isFound()); + assertEquals(3, result.getTotalCost()); + List path = result.getPath(); + + // Node 4 (sidewalk, risk=0.5) is preferred over node 1 (construction, risk=2.0) + assertFalse(path.contains(1), "Construction zone (node 1) should be avoided"); + assertTrue(path.contains(4), "Sidewalk (node 4) should be preferred"); + } + + @Test + public void testSemanticRiskOverridesShorterPath() { + // Strong semantic weight (lambda=5.0) makes the algorithm detour + // around construction zone despite longer actual distance + AdaptiveAStar.Graph graph = buildTestGraph(); + int[] heuristic = {3, 2, 1, 2, 1, 0}; + double[] semanticRisk = {0.0, 10.0, 0.0, 0.0, 0.0, 0.0}; + + AdaptiveAStar.PathResult result = AdaptiveAStar.findPath(0, 5, graph, heuristic, semanticRisk, 5.0); + + assertTrue(result.isFound()); + assertFalse(result.getPath().contains(1), + "Very high risk node should be avoided even if path is longer"); + } + + @Test + public void testStartNodeEqualsGoal() { + AdaptiveAStar.Graph graph = buildTestGraph(); + int[] heuristic = {0, 2, 1, 2, 1, 0}; + double[] semanticRisk = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + AdaptiveAStar.PathResult result = AdaptiveAStar.findPath(0, 0, graph, heuristic, semanticRisk, 0.0); + + assertTrue(result.isFound()); + assertEquals(0, result.getTotalCost()); + assertEquals(List.of(0), result.getPath()); + } + + @Test + public void testNoPathExists() { + // Two disconnected components + AdaptiveAStar.Graph graph = new AdaptiveAStar.Graph(4); + graph.addBidirectionalEdge(0, 1, 1); // component 1 + graph.addBidirectionalEdge(2, 3, 1); // component 2 + + int[] heuristic = {3, 2, 1, 0}; + double[] semanticRisk = {0.0, 0.0, 0.0, 0.0}; + + AdaptiveAStar.PathResult result = AdaptiveAStar.findPath(0, 3, graph, heuristic, semanticRisk, 0.0); + + assertFalse(result.isFound()); + assertEquals(-1, result.getTotalCost()); + } + + @Test + public void testInvalidNodeIndex() { + AdaptiveAStar.Graph graph = buildTestGraph(); + int[] heuristic = {3, 2, 1, 2, 1, 0}; + double[] semanticRisk = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + AdaptiveAStar.PathResult result = AdaptiveAStar.findPath(0, 100, graph, heuristic, semanticRisk, 1.0); + + assertFalse(result.isFound()); + assertEquals(-1, result.getTotalCost()); + } + + @Test + public void testZeroLambdaBehavesLikeClassicalAStar() { + // Verify that with lambda=0, risk values don't affect the result + AdaptiveAStar.Graph graph = buildTestGraph(); + int[] heuristic = {3, 2, 1, 2, 1, 0}; + + // High risk on all nodes, but lambda=0 so should be ignored + double[] highRisk = {5.0, 5.0, 5.0, 5.0, 5.0, 0.0}; + double[] noRisk = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + AdaptiveAStar.PathResult resultHighRisk = AdaptiveAStar.findPath(0, 5, graph, heuristic, highRisk, 0.0); + AdaptiveAStar.PathResult resultNoRisk = AdaptiveAStar.findPath(0, 5, graph, heuristic, noRisk, 0.0); + + assertTrue(resultHighRisk.isFound()); + assertTrue(resultNoRisk.isFound()); + assertEquals(resultNoRisk.getTotalCost(), resultHighRisk.getTotalCost()); + assertEquals(resultNoRisk.getPath(), resultHighRisk.getPath()); + } +}