I am trying to make 4-directional A-star pathfinding algorithm in C# but can't make it work properly. In Main method i have a example 5x5 int array map to set which fields can be accessed and which not (0 - clear field, 1 - obstacle). As example i set START point at (1, 3) and TARGET point at (4, 4), so i would expect path to avoid wall and proceed etc. but it doesn't. I've even made my algorithm display every parent and it's successors (neighbour). In my case it displays (2,4) as one of the nodes despite being marked as obstacle in Fields (Map), how did it happen? There is clear statement in GetSuccessors method to include only the ones marked as 0 but it still includes (2, 4) point. I am not really sure if something is wrong with GetSuccessors or FindPath (main algorithm).
Example map:
int[,] fields = new int[5, 5] //MAP, 1 - OBSTACLE
{
{ 0, 0, 0, 0, 0 },
{ 0, 1, 1, 1, 0 },
{ 0, 1, 1, 1, 0 },
{ 0, 0, 1, 0, 0 },
{ 0, 0, 1, 0, 0 }
};
Example points:
Node start = new Node(1, 3); //START LOCATION ON MAP
Node target = new Node(4, 4); //TARGET LOCATION ON MAP
Produced path (from TARGET to START):
4, 4
3, 4
2, 4
1, 3
Full code (FindPath and GetSuccessors are the main ones but i still could make something wrong with other methods):
using System;
using System.Collections.Generic;
using System.Linq;
namespace FoxLib
{
public class Node
{
public Node(int x, int y)
{
this.x = x;
this.y = y;
}
public int x, y; //POSITION ON MAP
public Node parent; //NEEDED TO RETRIEVE PATH LATER
public int G, H, F;
//G - COST FROM START NODE TO THIS NODE
//H - COST FROM THIS NODE TO TARGET (HEURISTIC)
//F - G + H; NODE COST
}
public class Pathfinding
{
public static void FindPath(int startX, int startY, int targetX, int targetY, int[,] fields) //MAIN ALGORITHM
{
bool pathFound=false; //IS PATH AVAILABLE?
Node start = new Node(startX, startY); //STARTING NODE
Node target = new Node(targetX, targetY); //TARGET NODE
start.parent = start;
start.G = 0;
start.H = Math.Abs(target.x - start.x) + Math.Abs(target.y - start.y);
start.F = start.G + start.H;
Node current = new Node(0, 0);
List<Node> openList = new List<Node>(); //NODES WAITING TO BE CHECKED
List<Node> closedList = new List<Node>(); //ALREADY CHECKED NODES
openList.Add(start);
while(openList.Count>0) //DO AS LONG AS THERE ARE STILL NODES TO DISCOVER
{
current = MinFNode(openList); //FIND NODE WITH LOWEST F COST (LOWER=BETTER PATH)
openList.Remove(current); //REMOVE CURRENT NODE FROM QUEUE
if ((current.x==target.x)&&(current.y==target.y)) //TARGET FOUND
{
Console.WriteLine("PATH FOUND");
pathFound = true;
//DISPLAY PATH (FROM TARGET TO START) BY CHECKING PARENTS
Console.WriteLine("[FULL PATH]");
do
{
Console.WriteLine(current.x + ", " + current.y);
current = current.parent;
} while ((current.x != start.x) && (current.y != start.y));
Console.WriteLine(start.x + ", " + start.y);
break;
}
List<Node> successors = GetSuccessors(current.x, current.y, fields); //GET SUCCESSORS
foreach (Node node in successors)
{
node.parent = current; //SET CURRENT NODE AS PARENT TO RETRIEVE PATH LATER
node.G = current.G + 10; //10 - DISTANCE FROM CURRENT TO ITS SUCCESSOR, current.G DISTANCE FROM CURRENT TO START
node.H = Math.Abs(target.x - node.x) + Math.Abs(target.y - node.y); //HEURISTIC DISTANCE TO TARGET
node.F = node.G + node.H; //NODE FINAL COST
Console.WriteLine(current.x + ", " + current.y + " PARENT | SUCCESSOR: " + node.x + ", " + node.y); //TEST DISPLAY TO CHECK SUCCESSORS CURRENT NODE PRODUCED
if (closedList.FirstOrDefault(l => l.x == node.x && l.y == node.y) != null) //IF SUCCESSOR ALREADY EXISTS ON CLOSED LIST
{
Node temp = closedList.FirstOrDefault(l => l.x == node.x && l.y == node.y);
if (node.F < temp.F) //IF NEW PATH TO THIS NODE IS BETTER? (LOWER F = SHORTER PATH)
{
closedList.Remove(temp); //REMOVE OLD NODE
temp.parent = node.parent;
temp.F = node.F;
closedList.Add(temp); //ADD UPDATED NODE
}
}
else
if(openList.FirstOrDefault(l => l.x == node.x && l.y == node.y) != null) //IF SUCCESSOR ALREADY EXISTS ON OPEN LIST
{
Node temp = openList.FirstOrDefault(l => l.x == node.x && l.y == node.y);
if (node.F < temp.F) //IF NEW PATH TO THIS NODE IS BETTER? (LOWER F = SHORTER PATH)
{
openList.Remove(temp); //REMOVE OLD NODE
temp.parent = node.parent;
temp.F = node.F;
openList.Add(temp); //ADD UPDATED NODE
}
}
else
{
openList.Add(node); //ADD SUCCESSOR TO OPEN LIST
}
}
closedList.Add(current); //MARK CURRENT NODE AS CHECKED (NO NEED TO CHECK IT UNTIL BETTER PATH TO THIS NODE IS FOUND)
}
if(!pathFound)
{
Console.WriteLine("PATH NOT FOUND");
}
}
//FIND NODE WITH LOWEST F COST
public static Node MinFNode(List<Node> nodes)
{
Node result = nodes[0];
foreach(Node node in nodes)
{
if (node.F < result.F)
result = node;
}
return result;
}
//GET SUCCESSORS OF CURRENT NODE (ONLY THE ONES WITH "0" VALUE)
public static List<Node> GetSuccessors(int x, int y, int[,] fields)
{
List<Node> Successors = new List<Node>();
if ((x - 1 >= 0) && (fields[x - 1, y]==0))
Successors.Add(new Node(x - 1, y)); //LEFT
if ((x + 1 < fields.GetLength(0)) && (fields[x + 1, y]==0))
Successors.Add(new Node(x + 1, y)); //RIGHT
if ((y - 1 >= 0) && (fields[x, y - 1]==0))
Successors.Add(new Node(x, y - 1)); //UP
if ((y + 1 < fields.GetLength(1)) && (fields[x, y + 1]==0))
Successors.Add(new Node(x, y + 1)); //DOWN
return Successors; //RETURN LIST OF AVAILABLE SUCCESSORS
}
static void Main()
{
int[,] fields = new int[5, 5] //MAP, 1 - OBSTACLE
{
{ 0, 0, 0, 0, 0 },
{ 0, 1, 1, 1, 0 },
{ 0, 1, 1, 1, 0 },
{ 0, 0, 1, 0, 0 },
{ 0, 0, 1, 0, 0 }
};
Node start = new Node(1, 3); //START LOCATION ON MAP
Node target = new Node(4, 4); //TARGET LOCATION ON MAP
FindPath(start.x,start.y,target.x,target.y,fields); //MAIN ALGORITHM
Console.WriteLine("END");
Console.ReadLine();
}
}
}
As Ivan pointed out, you're working from a false assumption. In a two-dimensional array the first index is the row number, not the column. This means that fields[2,4] is second row, fourth column... which is 0.
Simple fix... switch the indexing on all your fields references to fields[y, x] and you should be a step closer.
Eric's comment was perfectly valid however. You should have been able to pick this up with a little debugging. Step through the code and see what is actually happening and you'll get a much better insight into the problem. Since VS2015 Community Edition is free there's not much reason not to, and if you're not developing on Windows there are other C# IDEs that will allow you to step through your code and examine the state of the program as it operates. MonoDevelop is the go-to on Linux and Mac.