1+ import java .util .*;
2+
3+ class AStarSearch {
4+ static class Node implements Comparable <Node > {
5+ int x , y ;
6+ int g ;
7+ int h ;
8+ Node parent ;
9+
10+ Node (int x , int y , int g , int h , Node parent ) {
11+ this .x = x ;
12+ this .y = y ;
13+ this .g = g ;
14+ this .h = h ;
15+ this .parent = parent ;
16+ }
17+
18+ int f () {
19+ return g + h ;
20+ }
21+
22+ @ Override
23+ public int compareTo (Node other ) {
24+ return Integer .compare (this .f (), other .f ());
25+ }
26+
27+ @ Override
28+ public boolean equals (Object obj ) {
29+ if (!(obj instanceof Node )) return false ;
30+ Node other = (Node ) obj ;
31+ return this .x == other .x && this .y == other .y ;
32+ }
33+
34+ @ Override
35+ public int hashCode () {
36+ return Objects .hash (x , y );
37+ }
38+ }
39+
40+ private static int heuristic (int x1 , int y1 , int x2 , int y2 ) {
41+ return Math .abs (x1 - x2 ) + Math .abs (y1 - y2 );
42+ }
43+
44+ public static List <int []> aStar (int [][] grid , int [] start , int [] goal ) {
45+ int rows = grid .length , cols = grid [0 ].length ;
46+ PriorityQueue <Node > openSet = new PriorityQueue <>();
47+ Set <Node > closedSet = new HashSet <>();
48+
49+ Node startNode = new Node (start [0 ], start [1 ], 0 , heuristic (start [0 ], start [1 ], goal [0 ], goal [1 ]), null );
50+ openSet .add (startNode );
51+
52+ int [][] directions = {{0 ,1 },{1 ,0 },{0 ,-1 },{-1 ,0 }};
53+
54+ while (!openSet .isEmpty ()) {
55+ Node current = openSet .poll ();
56+
57+ if (current .x == goal [0 ] && current .y == goal [1 ]) {
58+ List <int []> path = new ArrayList <>();
59+ while (current != null ) {
60+ path .add (new int []{current .x , current .y });
61+ current = current .parent ;
62+ }
63+ Collections .reverse (path );
64+ return path ;
65+ }
66+
67+ closedSet .add (current );
68+
69+ for (int [] dir : directions ) {
70+ int nx = current .x + dir [0 ], ny = current .y + dir [1 ];
71+ if (nx < 0 || ny < 0 || nx >= rows || ny >= cols || grid [nx ][ny ] == 1 )
72+ continue ;
73+
74+ Node neighbor = new Node (nx , ny , current .g + 1 , heuristic (nx , ny , goal [0 ], goal [1 ]), current );
75+
76+ if (closedSet .contains (neighbor ))
77+ continue ;
78+
79+ boolean better = true ;
80+ for (Node node : openSet ) {
81+ if (node .equals (neighbor ) && node .g <= neighbor .g ) {
82+ better = false ;
83+ break ;
84+ }
85+ }
86+ if (better ) {
87+ openSet .add (neighbor );
88+ }
89+ }
90+ }
91+ return Collections .emptyList ();
92+ }
93+
94+ public static void main (String [] args ) {
95+ int [][] grid = {
96+ {0 , 0 , 0 , 0 , 0 },
97+ {1 , 1 , 0 , 1 , 0 },
98+ {0 , 0 , 0 , 1 , 0 },
99+ {0 , 1 , 1 , 0 , 0 },
100+ {0 , 0 , 0 , 0 , 0 }
101+ };
102+ int [] start = {0 , 0 };
103+ int [] goal = {4 , 4 };
104+
105+ List <int []> path = aStar (grid , start , goal );
106+ if (path .isEmpty ()) {
107+ System .out .println ("No path found." );
108+ } else {
109+ System .out .println ("Path:" );
110+ for (int [] p : path ) {
111+ System .out .println (Arrays .toString (p ));
112+ }
113+ }
114+ }
115+ }
0 commit comments