11
2-
3- import java .awt .Color ;
4- import java .awt .Graphics ;
5- import java .awt .Graphics2D ;
62import java .util .ArrayList ;
7-
83import java .util .PriorityQueue ;
9-
104import javax .swing .JPanel ;
115
12-
136public class AStarSearch extends JPanel {
147
158 private Tile [][] tiles ;
169 private Tile start ;
1710 private Tile goal ;
1811 private Ant ant ;
1912 private ArrayList <ArrayList <Tile >> allPath2D = new ArrayList <ArrayList <Tile >>();
20-
13+
2114 public AStarSearch (Tile [][] tiles , Ant ant , Tile start , Tile goal ) {
2215 this .tiles = tiles ;
2316 this .start = start ;
2417 this .goal = goal ;
2518 this .ant = ant ;
2619 }
27-
20+
2821 public ArrayList <Tile > search () {
2922
3023 ArrayList <Tile > output = new ArrayList <>();
3124 ArrayList <Tile > closedSet = new ArrayList <>();
32-
33- // create openSet and add start to it
25+
26+ // create openSet and add start to it
3427 PriorityQueue <Tile > openSet = new PriorityQueue <Tile >();
3528
3629 // add start to openSet
3730 openSet .add (start );
3831
3932 boolean foundGoal = false ;
4033 boolean noPath = false ;
41-
42-
43- // while openSet is not empty
44- while (openSet .size () > 0 && foundGoal == false && noPath == false ){
45-
34+
35+ // while openSet is not empty
36+ while (openSet .size () > 0 && foundGoal == false && noPath == false ) {
37+ // create a row for each iteration for drawing purposes
38+ ArrayList <Tile > row = new ArrayList <>();
39+
4640 // check if first element in openSet is goal
4741 Tile current = openSet .poll ();
48- // System.out.println("current: " + current);
49- ArrayList <Tile > row = new ArrayList <>();
5042 allPath2D .add (row );
5143 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 ){
44+
45+ if (current .equals (goal ) && noPath == false ) {
5646 // reconstruct path
5747 output = reconstructPath (current );
5848 foundGoal = true ;
5949 // ant.stopSearch();
6050
6151 } else {
62- // remove current from openSet and add to closedSet
63- // openSet.remove(current);
52+ // not goal, add to closedSet
6453 closedSet .add (current );
6554
6655 // get neighbors of current/ outgoing connections
6756 PriorityQueue <Tile > neighbors = getNeighbors (current );
6857
69- // printQueue(neighbors);
70-
71- // for each neighbor of current, evaluate cost
72- while (neighbors .size () > 0 ){
58+ // for each neighbor of current, evaluate cost
59+ while (neighbors .size () > 0 ) {
7360 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-
61+
7762 // if neighbor is not in closedSet and not obstacle, then evaluate cost
78- if (!closedSet .contains (currNeighbor ) && !currNeighbor .isObstacle ()){
63+ if (!closedSet .contains (currNeighbor ) && !currNeighbor .isObstacle ()) {
7964 row .add (currNeighbor );
8065 double tempCost = current .getG () + currNeighbor .getCost ();
81-
82- // System.out.println("tempCost: " + tempCost + " gCost: " + current.getG() + " current: " + current.getCost());
83-
84- boolean betterPath = false ;
8566
8667 // check for a shorter route
87- if (!openSet .contains (currNeighbor )){
68+ boolean betterPath = false ;
69+ if (!openSet .contains (currNeighbor )) {
8870 // if neighbor is not in openSet, add to openSet and add g cost
8971 currNeighbor .setG (tempCost );
9072 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 {
73+ } else {
9574 // openSet contains neighbor, check if shorter route
96-
97- if (tempCost < currNeighbor .getG ()){
98- // found a shorter route, update g cost
75+ if (tempCost < currNeighbor .getG ()) {
76+ // found a shorter route, update g cost then update the priority queue
9977 openSet .remove (currNeighbor );
10078 currNeighbor .setG (tempCost );
10179 openSet .add (currNeighbor );
10280 betterPath = true ;
103-
10481 }
10582 }
10683
107- // set neighbor's heuristic estimated cost to goal
108- if (betterPath ){
84+ // set neighbor's heuristic estimated cost to goal and f cost if a better path
85+ // is found
86+ if (betterPath ) {
10987 currNeighbor .setH (getHeuristic (currNeighbor , goal ));
11088
11189 // set neighbor's f
11290 currNeighbor .setF (currNeighbor .getG () + currNeighbor .getH ());
11391
11492 // record previous node
11593 currNeighbor .setCameFrom (current );
116-
94+
11795 }
118-
11996
12097 // 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());
98+ if (!openSet .contains (currNeighbor )) {
12399 openSet .add (currNeighbor );
124-
125100 }
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
132101
133102 } // end if statement
134- // System.out.println(" Grid ["+currNeighbor.getX()+"]["+currNeighbor.getY()+"] F:" + currNeighbor.getF() + " G:" + currNeighbor.getG() + " H:" + currNeighbor.getH());
135103
136104 } // end while loop
137105
138-
139-
140-
141106 } // end else(not goal)
142107
143108 // if openSet is empty, then no path
144- if (openSet .size () == 0 ){
109+ if (openSet .size () == 0 ) {
145110 System .out .println ("No path found" );
146111 noPath = true ;
147112 }
148-
149-
150-
113+
151114 } // end main while loop
152115
116+ // the list of all paths for drawing purposes
153117 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-
157118 return output ;
158119 }
159120
160-
161121 private double getHeuristic (Tile currNeighbor , Tile goal ) {
162122
163123 // manhattan distance between current neighbor and goal
@@ -171,47 +131,47 @@ private PriorityQueue<Tile> getNeighbors(Tile tile) {
171131 PriorityQueue <Tile > neighbors = new PriorityQueue <>();
172132
173133 // get the indexes of the tile from the neighbors 2d array
174-
134+
175135 int x = tile .getX ();
176136 int y = tile .getY ();
177137
178138 // add neighbours including diagonals
179139 // top left
180- if (x > 0 && y > 0 ){
181- neighbors .add (tiles [x - 1 ][y - 1 ]);
140+ if (x > 0 && y > 0 ) {
141+ neighbors .add (tiles [x - 1 ][y - 1 ]);
182142 }
183143 // top
184- if (y > 0 ){
185- neighbors .add (tiles [x ][y - 1 ]);
144+ if (y > 0 ) {
145+ neighbors .add (tiles [x ][y - 1 ]);
186146 }
187147 // top right
188- if (x < tiles .length - 1 && y > 0 ){
189- neighbors .add (tiles [x + 1 ][y - 1 ]);
148+ if (x < tiles .length - 1 && y > 0 ) {
149+ neighbors .add (tiles [x + 1 ][y - 1 ]);
190150 }
191151 // right
192- if (x < tiles .length - 1 ) {
193- neighbors .add (tiles [x + 1 ][y ]);
152+ if (x < tiles .length - 1 ) {
153+ neighbors .add (tiles [x + 1 ][y ]);
194154 }
195155 // bottom right
196- if (x < tiles .length - 1 && y < tiles [0 ].length - 1 ) {
197- neighbors .add (tiles [x + 1 ][y + 1 ]);
156+ if (x < tiles .length - 1 && y < tiles [0 ].length - 1 ) {
157+ neighbors .add (tiles [x + 1 ][y + 1 ]);
198158 }
199159 // bottom
200- if (y < tiles [0 ].length - 1 ) {
201- neighbors .add (tiles [x ][y + 1 ]);
160+ if (y < tiles [0 ].length - 1 ) {
161+ neighbors .add (tiles [x ][y + 1 ]);
202162 }
203163 // bottom left
204- if (x > 0 && y < tiles [0 ].length - 1 ) {
205- neighbors .add (tiles [x - 1 ][y + 1 ]);
164+ if (x > 0 && y < tiles [0 ].length - 1 ) {
165+ neighbors .add (tiles [x - 1 ][y + 1 ]);
206166 }
207167 // left
208- if (x > 0 ){
209- neighbors .add (tiles [x - 1 ][y ]);
168+ if (x > 0 ) {
169+ neighbors .add (tiles [x - 1 ][y ]);
210170 }
211171
212172 return neighbors ;
213173 }
214-
174+
215175 public ArrayList <Tile > reconstructPath (Tile current ) {
216176 ArrayList <Tile > path = new ArrayList <>();
217177
@@ -220,10 +180,9 @@ public ArrayList<Tile> reconstructPath(Tile current) {
220180
221181 // move to the previous node
222182 current = current .getCameFrom ();
223- // System.out.println("Path: (" + current.getX() + ", " + current.getY()+")");
224183 }
225184
226185 return path ;
227186 }
228-
187+
229188}
0 commit comments