Question

Write a program that implements the A* algorithm to find a path from any two given nodes. You may use any of the following languages: C++, C#, Java, ActionScript.

Problem Overview & Algorithm Description In a fully-observable environment where there are both pathable and blocked nodes, an agent must find a good path from their starting node to the goal node. The agent must use the A* algorithm to determine its path. For this program, you must use the Manhattan method for calculating the heuristic.

Remember: your heuristic function is a representation of how good or close you are to the goal state.

Program Requirements No graphics are required for this program but using them will help you with debugging and problem solving. Your environment should be a 15x15 tile-based world that randomly generates nodes that are unpathable (blocks) in 10% of the nodes. This should be done each time the program compiles ensuring that there are different environment makeups each run. The program should display the generated environment when the program runs, and should allow the user to select a starting node and goal node. This can be done via text input into the console or with a GUI. Once the start and goal nodes have been defined, the program should run the A* algorithm to find a path. The path should be displayed (series of [x,y] nodes, highlighting nodes, or actually moving the agent) if one exists, or a message indicating that a path could not be found. The user should be able to continue specifying starting and goal nodes after paths have been found.

(Here is some code that might help)

public class Node {

   private int row, col, f, g, h, type;
   private Node parent;

   public Node(int r, int c, int t) {
       row = r;
       col = c;
       type = t;
       parent = null;

       // type 0 is traverseable, 1 is not
   }

   // mutator methods to set values
   public void setF() {
       f = g + h;
   }

   public void setG(int value) {
       g = value;
   }

   public void setH(int value) {
       h = value;
   }

   public void setParent(Node n) {
       parent = n;
   }

   // accessor methods to get values
   public int getF() {
       return f;
   }

   public int getG() {
       return g;
   }

   public int getH() {
       return h;
   }

   public Node getParent() {
       return parent;
   }

   public int getRow() {
       return row;
   }

   public int getCol() {
       return col;
   }

   public boolean equals(Object in) {
       // typecast to Node
       Node n = (Node) in;
       return row == n.getRow() && col == n.getCol();
   }

   public String toString() {
       return "Node: " + row + "_" + col;

   }
}

Answer 1

A* algorithm is a graph traversal and path search algorithm. It is used for path-finding from one node to another.

Manhattan Distance is the sum of absolute values of differences in the goal’s x and y coordinates and the current cell’s x and y coordinates

h = abs (current_x_coordinate – goal_x_coordinate) + abs (current_y_coordinate – goal_y_coordinate)

C++ code:

#include <bits/stdc++.h>

using namespace std;

#define ROW 9

#define COL 10

typedef pair<int, int> Pair;        // Creating int, int pair type

typedef pair<double, pair<int, int>> pPair;     // Creating pair<int, pair<int, int>> type

struct cell               // A structure to hold the values

{

    int parent_of_i, parent_of_j;        

    double f, g, h;          

};

bool check_valid(int row_value, int col_value)  // A function to check whether a cell is valid or not

{

    return (row_value >= 0) && (row_value < ROW) && (col_value >= 0) && (col_value < COL);

}

bool Check_block(int matrix[][COL], int row_value, int col_value)   // function to check wether cell is blocked or not

{

    if (matrix[row_value][col_value] == 1)

        return (true);

    else

        return (false);

}

bool check_destination(int row_value, int col_value, Pair dest) // function to check whether destination has reached or not

{

    if (row_value == dest.first && col_value == dest.second)

        return (true);

    else

        return (false);

}

double calculate_heuristic(int row_value, int col_value, Pair dest) // Calculation of heuristic

{

    return ((double)sqrt((row_value - dest.first) * (row_value - dest.first) + (col_value - dest.second) * (col_value - dest.second)));

}

void tracePath(cell cell_detail[][COL], Pair dest)  //Function to trace the path from the sourceto destination

{

    printf("\nThe Path is ");

    int row_value = dest.first;

    int col_value = dest.second;

    stack<Pair> Path;

    while (!(cell_detail[row_value][col_value].parent_of_i == row_value && cell_detail[row_value][col_value].parent_of_j == col_value))

    {

        Path.push(make_pair(row_value, col_value));

        int temp_row = cell_detail[row_value][col_value].parent_of_i;

        int temp_col = cell_detail[row_value][col_value].parent_of_j;

        row_value = temp_row;

        col_value = temp_col;

    }

    Path.push(make_pair(row_value, col_value));

        while (!Path.empty())

        {

            pair<int, int> p = Path.top();

            Path.pop();

            printf("-> (%d,%d) ", p.first, p.second);   

        }

    return;

}

void func_a_search(int matrix[][COL], Pair source, Pair dest)   //Function to implement A* algorithm

{

    if (check_valid(source.first, source.second) == false)  // Check source is out of range

    {

        printf("Source is invalid\n");

        return;

    }

    if (check_valid(dest.first, dest.second) == false)  // Check destination is out of range

    {

        printf("destination is invalid\n");

        return;

    }

    if (Check_block(matrix, source.first, source.second) == false || Check_block(matrix, dest.first, dest.second) == false)

    {

        printf("Source or the destination is blocked\n");

        return;

    }

    if (check_destination(source.first, source.second, dest) == true) // If the destination cell is the same as source cell

    {

        printf("We are already at the destination\n");

        return;

    }

    bool closedList[ROW][COL];

    memset(closedList, false, sizeof(closedList));

    cell cell_detail[ROW][COL];

    int i, j;

    for (i = 0; i < ROW; i++)

    {

        for (j = 0; j < COL; j++)

        {

            cell_detail[i][j].f = FLT_MAX;

            cell_detail[i][j].g = FLT_MAX;

            cell_detail[i][j].h = FLT_MAX;

            cell_detail[i][j].parent_of_i = -1;

            cell_detail[i][j].parent_of_j = -1;

        }

    }

    i = source.first, j = source.second;    // Initialising the parameters of the starting node

    cell_detail[i][j].f = 0.0;

    cell_detail[i][j].g = 0.0;

    cell_detail[i][j].h = 0.0;

    cell_detail[i][j].parent_of_i = i;

    cell_detail[i][j].parent_of_j = j;

    set<pPair> open_List;

    open_List.insert(make_pair(0.0, make_pair(i, j)));

    bool founddest = false;

    while (!open_List.empty())

    {

        pPair p = *open_List.begin();

        open_List.erase(open_List.begin()); // Remove this vertex from the open list

        i = p.second.first; // Add this vertex to the closed list

        j = p.second.second;

        closedList[i][j] = true;

        double gNew, hNew, fNew;

        if (check_valid(i - 1, j) == true)

        {

            if (check_destination(i - 1, j, dest) == true)

            {

                cell_detail[i - 1][j].parent_of_i = i;  // Set the Parent of the destination cell

                cell_detail[i - 1][j].parent_of_j = j;

                printf("The destination cell is found\n");

                tracePath(cell_detail, dest);

                founddest = true;

                return;

            }

            else if (closedList[i - 1][j] == false && Check_block(matrix, i - 1, j) == true)

            {

                gNew = cell_detail[i][j].g + 1.0;

                hNew = calculate_heuristic(i - 1, j, dest);

                fNew = gNew + hNew;

                if (cell_detail[i - 1][j].f == FLT_MAX || cell_detail[i - 1][j].f > fNew)

                {

                    open_List.insert(make_pair(fNew,make_pair(i - 1, j)));

                    cell_detail[i - 1][j].f = fNew;// Update the details of this cell

                    cell_detail[i - 1][j].g = gNew;

                    cell_detail[i - 1][j].h = hNew;

                    cell_detail[i - 1][j].parent_of_i = i;

                    cell_detail[i - 1][j].parent_of_j = j;                

                }

            }

        }

        if (check_valid(i + 1, j) == true)

        {

            if (check_destination(i + 1, j, dest) == true)

            {

                cell_detail[i + 1][j].parent_of_i = i;  // Set the Parent of the destination cell

                cell_detail[i + 1][j].parent_of_j = j;

                printf("The destination cell is found\n");

                tracePath(cell_detail, dest);

                founddest = true;

                return;

            }

            else if (closedList[i + 1][j] == false && Check_block(matrix, i + 1, j) == true)

            {

                gNew = cell_detail[i][j].g + 1.0;

                hNew = calculate_heuristic(i + 1, j, dest);

                fNew = gNew + hNew;

                if (cell_detail[i + 1][j].f == FLT_MAX || cell_detail[i + 1][j].f > fNew)

                {

                    open_List.insert(make_pair(fNew, make_pair(i + 1, j))); // Update the details of this cell

                    

                    cell_detail[i + 1][j].f = fNew;

                    cell_detail[i + 1][j].g = gNew;

                    cell_detail[i + 1][j].h = hNew;

                    cell_detail[i + 1][j].parent_of_i = i;

                    cell_detail[i + 1][j].parent_of_j = j;

                }

            }

        }

        if (check_valid(i, j + 1) == true)

        {

            if (check_destination(i, j + 1, dest) == true)

            {

                cell_detail[i][j + 1].parent_of_i = i;  // Set the Parent of the destination cell

                cell_detail[i][j + 1].parent_of_j = j;

                printf("The destination cell is found\n");

                tracePath(cell_detail, dest);

                founddest = true;

                return;

            }

            else if (closedList[i][j + 1] == false && Check_block(matrix, i, j + 1) == true)

            {

                gNew = cell_detail[i][j].g + 1.0;

                hNew = calculate_heuristic(i, j + 1, dest);

                fNew = gNew + hNew;

                if (cell_detail[i][j + 1].f == FLT_MAX || cell_detail[i][j + 1].f > fNew)

                {

                    open_List.insert(make_pair(fNew,make_pair(i, j + 1)));

                    cell_detail[i][j + 1].f = fNew; // Update the details of this cell

                    cell_detail[i][j + 1].g = gNew;

                    cell_detail[i][j + 1].h = hNew;

                    cell_detail[i][j + 1].parent_of_i = i;

                    cell_detail[i][j + 1].parent_of_j = j;

                }

            }

        }

        if (check_valid(i, j - 1) == true)

        {

            if (check_destination(i, j - 1, dest) == true)

            {

                cell_detail[i][j - 1].parent_of_i = i;  // Set the Parent of the destination cell

                cell_detail[i][j - 1].parent_of_j = j;

                printf("The destination cell is found\n");

                tracePath(cell_detail, dest);

                founddest = true;

                return;

            }

            else if (closedList[i][j - 1] == false && Check_block(matrix, i, j - 1) == true)

            {   

                gNew = cell_detail[i][j].g + 1.0;

                hNew = calculate_heuristic(i, j - 1, dest);

                fNew = gNew + hNew;

                if (cell_detail[i][j - 1].f == FLT_MAX || cell_detail[i][j - 1].f > fNew)

                {

                    open_List.insert(make_pair(fNew,make_pair(i, j - 1)));

                    cell_detail[i][j - 1].f = fNew; // Update the details of this cell

                    cell_detail[i][j - 1].g = gNew;

                    cell_detail[i][j - 1].h = hNew;

                    cell_detail[i][j - 1].parent_of_i = i;

                    cell_detail[i][j - 1].parent_of_j = j;

                }

            }

        }

        if (check_valid(i - 1, j + 1) == true)

        {

            if (check_destination(i - 1, j + 1, dest) == true)

            {

                cell_detail[i - 1][j + 1].parent_of_i = i;  // Set the Parent of the destination cell

                cell_detail[i - 1][j + 1].parent_of_j = j;

                printf("The destination cell is found\n");

                tracePath(cell_detail, dest);

                founddest = true;

                return;

            }

            else if (closedList[i - 1][j + 1] == false && Check_block(matrix, i - 1, j + 1) == true)

            {

                gNew = cell_detail[i][j].g + 1.414;

                hNew = calculate_heuristic(i - 1, j + 1, dest);

                fNew = gNew + hNew;

                if (cell_detail[i - 1][j + 1].f == FLT_MAX || cell_detail[i - 1][j + 1].f > fNew)

                {

                    open_List.insert(make_pair(fNew,make_pair(i - 1, j + 1)));

                    cell_detail[i - 1][j + 1].f = fNew;// Update the details of this cell

                    cell_detail[i - 1][j + 1].g = gNew;

                    cell_detail[i - 1][j + 1].h = hNew;

                    cell_detail[i - 1][j + 1].parent_of_i = i;

                    cell_detail[i - 1][j + 1].parent_of_j = j;

                }   

            }

        }

        if (check_valid(i - 1, j - 1) == true)

        {

            if (check_destination(i - 1, j - 1, dest) == true)

            {

                cell_detail[i - 1][j - 1].parent_of_i = i;  // Set the Parent of the destination cell

                cell_detail[i - 1][j - 1].parent_of_j = j;

                printf("The destination cell is found\n");

                tracePath(cell_detail, dest);

                founddest = true;

                return;

            }

            else if (closedList[i - 1][j - 1] == false && Check_block(matrix, i - 1, j - 1) == true)

            {

                gNew = cell_detail[i][j].g + 1.414;

                hNew = calculate_heuristic(i - 1, j - 1, dest);

                fNew = gNew + hNew;

                if (cell_detail[i - 1][j - 1].f == FLT_MAX || cell_detail[i - 1][j - 1].f > fNew)

                {

                    open_List.insert(make_pair(fNew, make_pair(i - 1, j - 1)));     // Update the details of this cell

                    cell_detail[i - 1][j - 1].f = fNew;

                    cell_detail[i - 1][j - 1].g = gNew;

                    cell_detail[i - 1][j - 1].h = hNew;

                    cell_detail[i - 1][j - 1].parent_of_i = i;

                    cell_detail[i - 1][j - 1].parent_of_j = j;

                }

            }

        }

        if (check_valid(i + 1, j + 1) == true)  

        {

            if (check_destination(i + 1, j + 1, dest) == true)

            {

                

                cell_detail[i + 1][j + 1].parent_of_i = i;// Set the Parent of the destination cell

                cell_detail[i + 1][j + 1].parent_of_j = j;

                printf("The destination cell is found\n");

                tracePath(cell_detail, dest);

                founddest = true;

                return;

            }

            else if (closedList[i + 1][j + 1] == false && Check_block(matrix, i + 1, j + 1) == true)

            {

                gNew = cell_detail[i][j].g + 1.414;

                hNew = calculate_heuristic(i + 1, j + 1, dest);

                fNew = gNew + hNew;

                if (cell_detail[i + 1][j + 1].f == FLT_MAX || cell_detail[i + 1][j + 1].f > fNew)

                {

                    open_List.insert(make_pair(fNew,make_pair(i + 1, j + 1)));

                    cell_detail[i + 1][j + 1].f = fNew; // Update the details of this cell

                    cell_detail[i + 1][j + 1].g = gNew;

                    cell_detail[i + 1][j + 1].h = hNew;

                    cell_detail[i + 1][j + 1].parent_of_i = i;

                    cell_detail[i + 1][j + 1].parent_of_j = j;

                }

            }

        }

        if (check_valid(i + 1, j - 1) == true)

        {

            if (check_destination(i + 1, j - 1, dest) == true)

            {

                cell_detail[i + 1][j - 1].parent_of_i = i;// Set the Parent of the destination cell

                cell_detail[i + 1][j - 1].parent_of_j = j;

                printf("The destination cell is found\n");

                tracePath(cell_detail, dest);

                founddest = true;

                return;

            }

            else if (closedList[i + 1][j - 1] == false && Check_block(matrix, i + 1, j - 1) == true)

            {

                gNew = cell_detail[i][j].g + 1.414;

                hNew = calculate_heuristic(i + 1, j - 1, dest);

                fNew = gNew + hNew;

                if (cell_detail[i + 1][j - 1].f == FLT_MAX || cell_detail[i + 1][j - 1].f > fNew)

                {

                    open_List.insert(make_pair(fNew,make_pair(i + 1, j - 1)));

                    cell_detail[i + 1][j - 1].f = fNew;// Update the details of this cell

                    cell_detail[i + 1][j - 1].g = gNew;

                    cell_detail[i + 1][j - 1].h = hNew;

                    cell_detail[i + 1][j - 1].parent_of_i = i;

                    cell_detail[i + 1][j - 1].parent_of_j = j;

                }

            }

        }

    }

    if (founddest == false)

        printf("Failed to find the destination Cell\n");

    return;

}

int main()

{

    int matrix[ROW][COL] =

    {

        {1, 0, 1, 1, 1, 1, 0, 1, 1, 1},

        {1, 1, 1, 0, 1, 1, 1, 0, 1, 1},

        {1, 1, 1, 0, 1, 1, 0, 1, 0, 1},

        {0, 0, 1, 0, 1, 0, 0, 0, 0, 1},

        {1, 1, 1, 0, 1, 1, 1, 0, 1, 0},

        {1, 0, 1, 1, 1, 1, 0, 1, 0, 0},

        {1, 0, 0, 0, 0, 1, 0, 0, 0, 1},

        {1, 0, 1, 1, 1, 1, 0, 1, 1, 1},

        {1, 1, 1, 0, 0, 0, 1, 0, 0, 1}

    };

    Pair source = make_pair(8, 0);

    Pair dest = make_pair(0, 0);

    func_a_search(matrix, source, dest);

    return (0);

}