Algorithm2: A* search algorithm

Input: SN, GN, Q.

Output: best path from start node to goal node, if any

1. initialize the OL

2. initialize the CL

3. add SN to OL (you can leave its f at zero)

4. while (OL not empty)

5. find the node with the least f on the open list, call it "q"

6. pop q off OL

7. set q’s N parents to q

8. for (N_i)

9. if (N_i is the GN)

10. then stop the search

11. N_i.g = q.g + distance between N_i and q

12. N_i.h = distance from GN to N_i

13. N_i.f = N_i.g + N_i.h

14. if (Q_i’s position = N_i is in OL and Q_i.f

15. then skip N_i

16. if (Q_i’s position = N_i is in CL and Q_i.f

17. then skip N_i

18. else add N_i to OL

19. end

20. push q in the CL

30. end