B4J Question The A* Algorithm

Sub Process_Globals
    Type Node(x As Int, y As Int, g As Int, h As Int, f As Int, parent As Node)
    Private openList As List
    Private closedList As List
    Private grid(10, 10) As Int ' Example grid size
End Sub

Sub Globals
    ' Add any global variables here
End Sub

Sub Activity_Create(FirstTime As Boolean)
    openList.Initialize
    closedList.Initialize
    ' Initialize the grid with 0 (walkable) or 1 (blocked)
    ' For example:
    grid(5, 5) = 1 ' Block cell (5, 5)

    Dim startNode As Node = CreateNode(0, 0, 0, 0, 0, Null)
    Dim endNode As Node = CreateNode(9, 9, 0, 0, 0, Null)
    Dim path As List = AStar(startNode, endNode)
   
    If path.IsInitialized Then
        For Each node As Node In path
            Log("Path: (" & node.x & ", " & node.y & ")")
        Next
    Else
        Log("No path found")
    End If
End Sub

Sub CreateNode(x As Int, y As Int, g As Int, h As Int, f As Int, parent As Node) As Node
    Dim n As Node
    n.Initialize
    n.x = x
    n.y = y
    n.g = g
    n.h = h
    n.f = f
    n.parent = parent
    Return n
End Sub

Sub AStar(startNode As Node, endNode As Node) As List
    openList.Add(startNode)
   
    Do While openList.Size > 0
        Dim currentNode As Node = GetNodeWithLowestF(openList)
       
        If currentNode.x = endNode.x And currentNode.y = endNode.y Then
            Return ReconstructPath(currentNode)
        End If
       
        openList.RemoveAt(openList.IndexOf(currentNode))
        closedList.Add(currentNode)
       
        Dim neighbors As List = GetNeighbors(currentNode)
       
        For Each neighbor As Node In neighbors
            If IsInList(closedList, neighbor) Then Continue
           
            Dim tentative_gScore As Int = currentNode.g + 1 ' Assuming cost from current to neighbor is 1
           
            If Not IsInList(openList, neighbor) Then
                openList.Add(neighbor)
            ElseIf tentative_gScore >= neighbor.g Then
                Continue
            End If
           
            neighbor.parent = currentNode
            neighbor.g = tentative_gScore
            neighbor.h = HeuristicCostEstimate(neighbor, endNode)
            neighbor.f = neighbor.g + neighbor.h
        Next
    Loop
   
    Return Null ' No path found
End Sub

Sub GetNodeWithLowestF(list As List) As Node
    Dim lowestFNode As Node = list.Get(0)
    For Each n As Node In list
        If n.f < lowestFNode.f Then
            lowestFNode = n
        End If
    Next
    Return lowestFNode
End Sub

Sub GetNeighbors(node As Node) As List
    Dim neighbors As List
    neighbors.Initialize
   
    Dim directions() As Int = Array As Int(-1, 0, 1, 0, 0, -1, 0, 1) ' Left, Right, Up, Down
   
    For i = 0 To directions.Length - 1 Step 2
        Dim nx As Int = node.x + directions(i)
        Dim ny As Int = node.y + directions(i + 1)
       
        If nx >= 0 And nx < grid.Length And ny >= 0 And ny < grid(0).Length Then
            If grid(nx, ny) = 0 Then ' Check if the cell is walkable
                neighbors.Add(CreateNode(nx, ny, 0, 0, 0, node))
            End If
        End If
    Next
   
    Return neighbors
End Sub

Sub IsInList(list As List, node As Node) As Boolean
    For Each n As Node In list
        If n.x = node.x And n.y = node.y Then Return True
    Next
    Return False
End Sub

Sub HeuristicCostEstimate(node As Node, endNode As Node) As Int
    ' Using Manhattan distance as the heuristic
    Return Abs(node.x - endNode.x) + Abs(node.y - endNode.y)
End Sub

Sub ReconstructPath(currentNode As Node) As List
    Dim totalPath As List
    totalPath.Initialize
    totalPath.Add(currentNode)
   
    Do While currentNode.parent <> Null
        currentNode = currentNode.parent
        totalPath.InsertAt(0, currentNode)
    Loop
   
    Return totalPath
End Sub

Sub AppStart (Args() As String)
    Dim aastar As JavaObject
    aastar.InitializeStatic("b4j.example.main.AAsterisk")

    aastar.RunMethod("main",Null)
End Sub
#if java
import java.util.PriorityQueue;
import java.util.Stack;

public static class AAsterisk {

//Java Program to implement A* Search Algorithm

   //Here we're creating a shortcut for (int, int) pair
   public static class Pair {
       int first;
       int second;
       public Pair(int first, int second){
           this.first = first;
           this.second = second;
       }

       public boolean equals(Pair obj) {
           return this.first == obj.first && this.second == obj.second;
       }
   }

   // Creating a shortcut for tuple<int, int, int> type
   public static class Details {
       double value;
       int i;
       int j;

       public Details(double value, int i, int j) {
           this.value = value;
           this.i = i;
           this.j = j;
       }
   }

   // a Cell (node) structure
   public static class Cell {
       public Pair parent;
       // f = g + h, where h is heuristic
       public double f, g, h;
       Cell()
       {
           parent = new Pair(-1, -1);
           f = -1;
           g = -1;
           h = -1;
       }

       public Cell(Pair parent, double f, double g, double h) {
           this.parent = parent;
           this.f = f;
           this.g = g;
           this.h = h;
       }
   }

   // method to check if our cell (row, col) is valid
   boolean isValid(int[][] grid, int rows, int cols,
                   Pair point)
   {
       if (rows > 0 && cols > 0)
           return (point.first >= 0) && (point.first < rows)
                   && (point.second >= 0)
                   && (point.second < cols);

       return false;
   }

   //is the cell blocked?

   boolean isUnBlocked(int[][] grid, int rows, int cols,
                       Pair point)
   {
       return isValid(grid, rows, cols, point)
               && grid[point.first][point.second] == 1;
   }

   //Method to check if destination cell has been already reached
   boolean isDestination(Pair position, Pair dest)
   {
       return position == dest || position.equals(dest);
   }

   // Method to calculate heuristic function
   double calculateHValue(Pair src, Pair dest)
   {
       return Math.sqrt(Math.pow((src.first - dest.first), 2.0) + Math.pow((src.second - dest.second), 2.0));
   }

   // Method for tracking the path from source to destination

   void tracePath(
           Cell[][] cellDetails,
           int cols,
           int rows,
           Pair dest)
   {   //A* Search algorithm path
       System.out.print("The Path:  ");

       Stack<Pair> path = new Stack<>();

       int row = dest.first;
       int col = dest.second;

       Pair nextNode = cellDetails[row][col].parent;
       do {
           path.push(new Pair(row, col));
           nextNode = cellDetails[row][col].parent;
           row = nextNode.first;
           col = nextNode.second;
       } while (cellDetails[row][col].parent != nextNode); // until src

       boolean first = true;    
       while (!path.empty()) {
           Pair p = path.peek();
           path.pop();
           if (first) {
                   System.out.print( "(" + p.first + "," + p.second + ")");
                   first = !first;
           } else {
                   System.out.print("->(" + p.first + "," + p.second + ")");
           }
       }
       System.out.println();
   }

// A main method, A* Search algorithm to find the shortest path

   void aStarSearch(int[][] grid,
                    int rows,
                    int cols,
                    Pair src,
                    Pair dest)
   {

       if (!isValid(grid, rows, cols, src)) {
           System.out.println("Source is invalid...");
           return;
       }


       if (!isValid(grid, rows, cols, dest)) {
           System.out.println("Destination is invalid...");
           return;
       }


       if (!isUnBlocked(grid, rows, cols, src)
               || !isUnBlocked(grid, rows, cols, dest)) {
           System.out.println("Source or destination is blocked...");
           return;
       }


       if (isDestination(src, dest)) {
           System.out.println("We're already (t)here...");
           return;
       }


       boolean[][] closedList = new boolean[rows][cols];//our closed list

       Cell[][] cellDetails = new Cell[rows][cols];

       int i, j;
       // Initialising of the starting cell
       i = src.first;
       j = src.second;
       cellDetails[i][j] = new Cell();
       cellDetails[i][j].f = 0.0;
       cellDetails[i][j].g = 0.0;
       cellDetails[i][j].h = 0.0;
       cellDetails[i][j].parent = new Pair( i, j );


  // Creating an open list


       PriorityQueue<Details> openList = new PriorityQueue<>((o1, o2) -> (int) Math.round(o1.value - o2.value));

       // Put the starting cell on the open list,   set f.startCell = 0

       openList.add(new Details(0.0, i, j));

       while (!openList.isEmpty()) {
           Details p = openList.peek();
           // Add to the closed list
           i = p.i; // second element of tuple
           j = p.j; // third element of tuple

           // Remove from the open list
           openList.poll();
           closedList[i][j] = true;

              // Generating all the 8 neighbors of the cell

           for (int addX = -1; addX <= 1; addX++) {
               for (int addY = -1; addY <= 1; addY++) {
                   Pair neighbour = new Pair(i + addX, j + addY);
                   if (isValid(grid, rows, cols, neighbour)) {
                       if(cellDetails[neighbour.first] == null){ cellDetails[neighbour.first] = new Cell[cols]; }
                       if (cellDetails[neighbour.first][neighbour.second] == null) {
                           cellDetails[neighbour.first][neighbour.second] = new Cell();
                       }

                       if (isDestination(neighbour, dest)) {
                           cellDetails[neighbour.first][neighbour.second].parent = new Pair ( i, j );
                           System.out.println("The destination cell is found");
                           tracePath(cellDetails, rows, cols, dest);
                           return;
                       }

                       else if (!closedList[neighbour.first][neighbour.second]
                               && isUnBlocked(grid, rows, cols, neighbour)) {
                           double gNew, hNew, fNew;
                           gNew = cellDetails[i][j].g + 1.0;
                           hNew = calculateHValue(neighbour, dest);
                           fNew = gNew + hNew;

                           if (cellDetails[neighbour.first][neighbour.second].f == -1
                                   || cellDetails[neighbour.first][neighbour.second].f > fNew) {

                               openList.add(new Details(fNew, neighbour.first, neighbour.second));

                               // Update the details of this
                               // cell
                               cellDetails[neighbour.first][neighbour.second].g = gNew;
//heuristic function                               cellDetails[neighbour.first][neighbour.second].h = hNew;
                               cellDetails[neighbour.first][neighbour.second].f = fNew;
                               cellDetails[neighbour.first][neighbour.second].parent = new Pair( i, j );
                           }
                       }
                   }
               }
           }
       }

       System.out.println("Failed to find the Destination Cell");
   }

   // test
   public static void main() {


   //0: The cell is blocked
  // 1: The cell is not blocked

       int[][] grid = {
               { 1, 1, 0, 0, 1, 0, 0, 0 },
               { 1, 0, 0, 1, 1, 1, 1, 0 },
               { 1, 1, 0, 1, 0, 0, 1, 0 },
               { 1, 0, 0, 1, 1, 1, 1, 1 },
               { 1, 1, 0, 1, 0, 1, 1, 1 },
               { 0, 1, 1, 1, 0, 1, 1, 0 },
               { 1, 1, 0, 1, 1, 1, 1, 0 },
               { 0, 1, 1, 1, 1, 1, 1, 1 }

       };


       // Start is the left-most upper-most corner
       Pair src = new Pair(0,0);
               //(8, 0);

       // Destination is the right-most bottom-most corner
       //Pair dest = new Pair(6, 6);
         Pair dest = new Pair(3, 6);
         
       AAsterisk app = new AAsterisk();
       app.aStarSearch(grid, grid.length , grid[0].length, src, dest);

   }
}
#End If