Yes, data structures would help you a lot on this sort of problem. I'll try to explain how A* works and give some better Pseudocode afterwards.
A* is a Best-First search algorithm. This means that it's supposed to guess which options are best, and try to explore those first. This requires you to keep track of a list of options, typically called the "Front" (as in front-line). It doesn't keep track of a path found so far, like in your current algorithm. The algorithm works in two phases...
Phase 1
Basically, you start from the starting position S
, and all the neighbouring positions (north, west, south and east) will be in the Front. The algorithm then finds the most promising of the options in the Front (let's call it P
), and expands on that. The position P
is removed from the Front, but all of its neighbours are added in stead. Well, not all of its neighbours; only the neighbours that are actual options to go. We can't go walking into a tower, and we wouldn't want to go back to a place we've seen before. From the new Front, the most promising option is chosen, and so on. When the most promising option is the goal C
, the algorithm stops and enters phase 2.
Normally, the most promising option would be the one that is closest to the goal, as the crow flies (ignoring obstacles). So normally, it would always explore the one that is closest to the goal first. This causes the algorithm to walk towards the goal in a sort-of straight line. However, if that line is blocked by some obstacle, the positions of the obstacle should not be added to the Front. They are not viable options. So in the next round then, some other position in the Front would be selected as the best option, and the search continues from there. That is how it gets out of dead ends like the one in your example. Take a look at this illustration to get what I mean: https://upload.wikimedia.org/wikipedia/commons/5/5d/Astar_progress_animation.gif The Front is the hollow blue dots, and they mark dots where they've already been in a shade from red to green, and impassable places with thick blue dots.
In phase 2, we will need some extra information to help us find the shortest path back when we found the goal. For this, we store in every position the position we came from. If the algorithm works, the position we came from necessarily is closer to S
than any other neighbour. Take a look at the pseudocode below if you don't get what I mean.
Phase 2
When the castle C
is found, the next step is to find your way back to the start, gathering what was the best path. In phase 1, we stored the position we came from in every position that we explored. We know that this position must always be closer to S
(not ignoring obstacles). The task in phase 2 is thus very simple: Follow the way back to the position we came from, every time, and keep track of these positions in a list. At the end, you'll have a list that forms the shortest path from C
to S
. Then you simply need to reverse this list and you have your answer.
I'll give some pseudocode to explain it. There are plenty of real code examples (in Java too) on the internet. This pseudocode assumes you use a 2D array to represent the grid. An alternative would be to have Node objects, which is simpler to understand in Pseudocode but harder to program and I suspect you'd use a 2D array anyway.
//Phase 1
origins = new array[gridLength][gridWidth]; //Keeps track of 'where we came from'.
front = new Set(); //Empty set. You could use an array for this.
front.add(all neighbours of S);
while(true) { //This keeps on looping forever, unless it hits the "break" statement below.
best = findBestOption(front);
front.remove(best);
for(neighbour in (best's neighbours)) {
if(neighbour is not a tower and origins[neighbour x][neighbour y] == null) { //Not a tower, and not a position that we explored before.
front.add(neighbour);
origins[neighbour x][neighbour y] = best;
}
}
if(best == S) {
break; //Stops the loop. Ends phase 1.
}
}
//Phase 2
bestPath = new List(); //You should probably use Java's ArrayList class for this if you're allowed to do that. Otherwise select an array size that you know is large enough.
currentPosition = C; //Start at the endpoint.
bestPath.add(C);
while(currentPosition != S) { //Until we're back at the start.
currentPosition = origins[currentPosition.x][currentPosition.y];
bestPath.add(currentPosition);
}
bestPath.reverse();
And for the findBestOption
method in that pseudocode:
findBestOption(front) {
bestPosition = null;
distanceOfBestPosition = Float.MAX_VALUE; //Some very high number to start with.
for(position in front) {
distance = Math.sqrt(position.x * position.x - C.x * C.x + position.y * position.y - C.y * C.y); //Euclidean distance (Pythagoras Theorem). This does the diagonal thing for you.
if(distance < distanceOfBestPosition) {
distanceOfBestPosition = distance;
bestPosition = position;
}
}
}
I hope this helps. Feel free to ask on!