1+
2+
3+ import java .awt .Color ;
4+ import java .awt .Graphics ;
5+ import java .awt .Graphics2D ;
6+ import java .util .ArrayList ;
7+
8+ import java .util .PriorityQueue ;
9+
10+ import javax .swing .JPanel ;
11+
12+
13+ public class AStarSearch extends JPanel {
14+
15+ private Tile [][] tiles ;
16+ private Tile start ;
17+ private Tile goal ;
18+ private Ant ant ;
19+ private ArrayList <ArrayList <Tile >> allPath2D = new ArrayList <ArrayList <Tile >>();
20+
21+ public AStarSearch (Tile [][] tiles , Ant ant , Tile start , Tile goal ) {
22+ this .tiles = tiles ;
23+ this .start = start ;
24+ this .goal = goal ;
25+ this .ant = ant ;
26+ }
27+
28+ public ArrayList <Tile > search () {
29+
30+ ArrayList <Tile > output = new ArrayList <>();
31+ ArrayList <Tile > closedSet = new ArrayList <>();
32+
33+ // create openSet and add start to it
34+ PriorityQueue <Tile > openSet = new PriorityQueue <Tile >();
35+
36+ // add start to openSet
37+ openSet .add (start );
38+
39+ boolean foundGoal = false ;
40+ boolean noPath = false ;
41+
42+
43+ // while openSet is not empty
44+ while (openSet .size () > 0 && foundGoal == false && noPath == false ){
45+
46+ // check if first element in openSet is goal
47+ Tile current = openSet .poll ();
48+ // System.out.println("current: " + current);
49+ ArrayList <Tile > row = new ArrayList <>();
50+ allPath2D .add (row );
51+ row .add (current );
52+ // System.out.println();
53+ // System.out.println("Removing Main Grid ["+current.getX()+"]["+current.getY()+"] F:" + current.getF() + " G:" + current.getG() + " H:" + current.getH());
54+
55+ if (current .equals (goal ) && noPath == false ){
56+ // reconstruct path
57+ output = reconstructPath (current );
58+ foundGoal = true ;
59+ // ant.stopSearch();
60+
61+ } else {
62+ // remove current from openSet and add to closedSet
63+ // openSet.remove(current);
64+ closedSet .add (current );
65+
66+ // get neighbors of current/ outgoing connections
67+ PriorityQueue <Tile > neighbors = getNeighbors (current );
68+
69+ // printQueue(neighbors);
70+
71+ // for each neighbor of current, evaluate cost
72+ while (neighbors .size () > 0 ){
73+ Tile currNeighbor = neighbors .poll ();
74+
75+ // System.out.println("--Polled Grid ["+currNeighbor.getX()+"]["+currNeighbor.getY()+"] F:" + currNeighbor.getF() + " G:" + currNeighbor.getG() + " H:" + currNeighbor.getH());
76+
77+ // if neighbor is not in closedSet and not obstacle, then evaluate cost
78+ if (!closedSet .contains (currNeighbor ) && !currNeighbor .isObstacle ()){
79+ row .add (currNeighbor );
80+ double tempCost = current .getG () + currNeighbor .getCost ();
81+
82+ // System.out.println("tempCost: " + tempCost + " gCost: " + current.getG() + " current: " + current.getCost());
83+
84+ boolean betterPath = false ;
85+
86+ // check for a shorter route
87+ if (!openSet .contains (currNeighbor )){
88+ // if neighbor is not in openSet, add to openSet and add g cost
89+ currNeighbor .setG (tempCost );
90+ betterPath = true ;
91+
92+ // System.out.println("ADDING Grid ["+currNeighbor.getX()+"]["+currNeighbor.getY()+"] F:" + currNeighbor.getF() + " G:" + currNeighbor.getG() + " H:" + currNeighbor.getH());
93+
94+ } else {
95+ // openSet contains neighbor, check if shorter route
96+
97+ if (tempCost < currNeighbor .getG ()){
98+ // found a shorter route, update g cost
99+ openSet .remove (currNeighbor );
100+ currNeighbor .setG (tempCost );
101+ openSet .add (currNeighbor );
102+ betterPath = true ;
103+
104+ }
105+ }
106+
107+ // set neighbor's heuristic estimated cost to goal
108+ if (betterPath ){
109+ currNeighbor .setH (getHeuristic (currNeighbor , goal ));
110+
111+ // set neighbor's f
112+ currNeighbor .setF (currNeighbor .getG () + currNeighbor .getH ());
113+
114+ // record previous node
115+ currNeighbor .setCameFrom (current );
116+
117+ }
118+
119+
120+ // add neighbor to openSet if not already in it
121+ if (!openSet .contains (currNeighbor )){
122+ // System.out.println("ADDING Grid ["+currNeighbor.getX()+"]["+currNeighbor.getY()+"] F:" + currNeighbor.getF() + " G:" + currNeighbor.getG() + " H:" + currNeighbor.getH());
123+ openSet .add (currNeighbor );
124+
125+ }
126+
127+
128+ // System.out.println("--Polled Grid ["+currNeighbor.getX()+"]["+currNeighbor.getY()+"] F:" + currNeighbor.getF() + " G:" + currNeighbor.getG() + " H:" + currNeighbor.getH());
129+
130+
131+ // update GUI after each iteration of the search algorithm
132+
133+ } // end if statement
134+ // System.out.println(" Grid ["+currNeighbor.getX()+"]["+currNeighbor.getY()+"] F:" + currNeighbor.getF() + " G:" + currNeighbor.getG() + " H:" + currNeighbor.getH());
135+
136+ } // end while loop
137+
138+
139+
140+
141+ } // end else(not goal)
142+
143+ // if openSet is empty, then no path
144+ if (openSet .size () == 0 ){
145+ System .out .println ("No path found" );
146+ noPath = true ;
147+ }
148+
149+
150+
151+ } // end main while loop
152+
153+ ant .setAllPath2D (allPath2D );
154+ // System.out.println("Grid [1][0] F:" + tiles[1][0].getF() + " G:" + tiles[1][0].getG() + " H:" + tiles[1][0].getH());
155+ // System.out.println("Grid [1][1] F:" + tiles[1][1].getF() + " G:" + tiles[1][1].getG() + " H:" + tiles[1][1].getH());
156+
157+ return output ;
158+ }
159+
160+
161+ private double getHeuristic (Tile currNeighbor , Tile goal ) {
162+
163+ // manhattan distance between current neighbor and goal
164+ int dx = Math .abs (currNeighbor .getX () - goal .getX ());
165+ int dy = Math .abs (currNeighbor .getY () - goal .getY ());
166+ return dx + dy ;
167+ }
168+
169+ private PriorityQueue <Tile > getNeighbors (Tile tile ) {
170+
171+ PriorityQueue <Tile > neighbors = new PriorityQueue <>();
172+
173+ // get the indexes of the tile from the neighbors 2d array
174+
175+ int x = tile .getX ();
176+ int y = tile .getY ();
177+
178+ // add neighbours including diagonals
179+ // top left
180+ if (x > 0 && y > 0 ){
181+ neighbors .add (tiles [x -1 ][y -1 ]);
182+ }
183+ // top
184+ if (y > 0 ){
185+ neighbors .add (tiles [x ][y -1 ]);
186+ }
187+ // top right
188+ if (x < tiles .length -1 && y > 0 ){
189+ neighbors .add (tiles [x +1 ][y -1 ]);
190+ }
191+ // right
192+ if (x < tiles .length -1 ){
193+ neighbors .add (tiles [x +1 ][y ]);
194+ }
195+ // bottom right
196+ if (x < tiles .length -1 && y < tiles [0 ].length -1 ){
197+ neighbors .add (tiles [x +1 ][y +1 ]);
198+ }
199+ // bottom
200+ if (y < tiles [0 ].length -1 ){
201+ neighbors .add (tiles [x ][y +1 ]);
202+ }
203+ // bottom left
204+ if (x > 0 && y < tiles [0 ].length -1 ){
205+ neighbors .add (tiles [x -1 ][y +1 ]);
206+ }
207+ // left
208+ if (x > 0 ){
209+ neighbors .add (tiles [x -1 ][y ]);
210+ }
211+
212+ return neighbors ;
213+ }
214+
215+ public ArrayList <Tile > reconstructPath (Tile current ) {
216+ ArrayList <Tile > path = new ArrayList <>();
217+
218+ while (current != null ) {
219+ path .add (current );
220+
221+ // move to the previous node
222+ current = current .getCameFrom ();
223+ // System.out.println("Path: (" + current.getX() + ", " + current.getY()+")");
224+ }
225+
226+ return path ;
227+ }
228+
229+ }
0 commit comments