Implementing A* algorithm

Question

So I am implementing A* algorithm in C. Here's the procedure.

I am using Priority Queue [using array] for all the open nodes. Since I'll have duplicate distances, that is more than one node with same distance/Priority, hence while inserting a node in PQ, if the parent of the inserted node has the same priority, I still swap them both, so that my newest entered member remains on the top( or as high as possible),so that I keep following a particular direction. Also, on removing, when I swap the topmost element with the last one, then again, if the swapped last element has the same as one of its children, then it gets swapped to the bottom.(I am not sure if this will affect in any way).

Now the problem is say I have a 100*100 matrix, and I have obstacles from (0,20) to (15,20) of the 2D array , in which I am moving. Now for a starting position (2,2) and ending position (16,20) I get a straight path, i.e. firstly go all the way to right, then go down till 15 then move one right and I am done.

But, if I have starting as (2,2) and last as (12,78) i.e. the points are separated by the obstacles and the path has to go around it, I still go via (16,20) and my path after (16,20) is still straight, but my path upto (16,20) is zig zag, i.e. I go some distance straight down, then some right, then down then right and so on, ultimately reaching (16,20) and going straight after that.

Why this zig zag path for the first half of the distance, what can I do to make sure that my path is straight, as it is, when my destination is (16,20) and not (12,78).

Thanks.

void findPath(array[ROW][COLUMN],sourceX,sourceY,destX,destY) {
  PQ pq[SIZE];
  int x,y;

  insert(pq,sourceX,sourceY);

  while(!empty(pq)) {
    remove(pq);
    if(removedIsDestination)
        break;                  //Path Found
    insertAdjacent(pq,x,y,destX,destY);
  }
}

void insert(PQ pq[SIZE],element){
  ++sizeOfPQ;
  PQ[sizeOfPQ]==element
  int i=sizeOfPQ;
  while(i>0){
    if(pq[i].priority <= pq[(i-1)/2].priority){
      swapWithParent
      i=(i-1)/2;
    }
    else
      break;
  }
}

Answer 1

You should change your scoring part. Right now you calculate absolute distance. Instead calculate min move distance. If you count each move as one then if you were at (x,y) and going to (dX,dY) that would be

distance moved + (max(x,dX) - min(x,dx) + max(y,dY) - min(y,dY))

A lower value is considered a higher score.

This heuristic is a guess at how many moves it would take if there was nothing in the way.

---

The nice thing about the heuristic is you can change it to get the results you want, for example if you prefer to move in a straight line as you suggest, then you can make this change:

= distance moved + (max(x,dX) - min(x,dx) + max(y,dY) - min(y,dY)) 
     + (1 if this is a turn from the last move)

This will cause you to "find" solutions which tend to go in the same direction.

If you want to FORCE as few turns as possible:

= distance moved + (max(x,dX) - min(x,dx) + max(y,dY) - min(y,dY)) 
     + (1 times the number of turns made)

This is what is nice about A* -- the heuristic will inform the search -- you will still always find a solution, but if there is more than one you can influence where you look first -- this makes it good for simulating AI behavior.

---

Doubt : How is the first one and second calculating way different from each other?

The first one puts a lower priority on a move that is a turn. The second one puts a lower priority on a path with more turns. In some cases (eg, the first turn) the value will be the same, but over all the 2nd one will pick paths that have as few turns as possible, where the first one might not.

Also, 1 if this is a turn from the last move , for this, say i have source at top left and destination at bottom right, now my path normally would be, left,left,left...down,down,down.... Now, 1 if this is a turn from the last move, according to this, when I change from left to down, will I add 1?

Yes

Wont it make the total value more and the priority for down will decrease.

Yes, exactly. You want to not look at choices that have a turn in them first. This will make them lower priority and your algorithm will investigate other options with a higher priority -- exactly what you want.

Or 1 if this is a turn from the last move is when I move to a cell, that is not abutting the cell previously worked upon? Thnks –

No, I don't understand this question -- I don't think it makes sense in this context -- all moves have to abut the previous cell, diagonal moves are not allowed.

---

Though, I'd really appreciate if you could tell me one instance where the first and second methods will give different answers. If you could. Thanks alot. :) 

Not so easy without seeing the details of your algorithm but the following might work:

The red are blocks. The green is what I would expect the first one to do, it locally tries to find the least turn. The blue is the least turn solution. Note, how far the red areas are from each other and the details of how your algorithm influence if this will work. As I have it above -- having an extra turn only costs 1 in the heuristic. SO, if you want to be sure this will work change the heuristic like this:

= distance moved + (max(x,dX) - min(x,dx) + max(y,dY) - min(y,dY)) 
     + (25 times the number of turns made)

Where 25 is bigger than the distance to get past the 2nd turn in the green path. (Thus after the 2nd turn the blue path will be searched.)