public List <Structs.Node> findPath(int startX, int startY, int endX, int endY, int[,] mapMatrix)
{
int i = 0;
bool pathFound = false;
int currentH = Math.Abs(startX - startY) + Math.Abs(endX - endY);
List <Structs.Node> path = new List <Structs.Node>();
Structs.Node currentTile = new Structs.Node();
currentTile = researchNode(startX, startY, endX, endY, i, mapMatrix, -1);
openList.Add(currentTile);
while (!pathFound)
{
float lowestH = 65535;
// When we don't have a path, find the lowest H value in our openList
foreach (Structs.Node nd in openList)
{
// If its walkable and has lowest H value, make it the next tile to be examined.
if (nd.heuristic < lowestH && nd.walkable)
{
currentTile = nd;
lowestH = currentTile.heuristic;
}
// Else if it's not walkable, remove from open and add to closed list.
else if (!nd.walkable)
{
openList.Remove(nd);
closedList.Add(nd);
}
}
// Now research the tile around the selected node.
// For each value where the X coord is within 1 sqm of the node, as is the Y coord
for (int x = -1; x < 2; x++)
{
for (int y = -1; y < 2; y++)
{
if (x == 0 && y == 0)
{
}
else
{
try
{
if (mapMatrix[currentTile.X + x, currentTile.Y + y] == 2)
{
pathFound = true;
}
// Add the researched tile to the open list.
openList.Add(researchNode(currentTile.X + x, currentTile.Y + y, endX, endY, i, mapMatrix, currentTile.ID));
}
catch (Exception ex)
{
}
}
}
}
openList.Remove(currentTile);
}
return(openList);
}
// Function FindPath
public Structs.Node GetLowestF(List <Structs.Node> nList)
{
float currentLowest = 65535;
Structs.Node currentNode = nList[0];
foreach (Structs.Node node in nList)
{
if (currentLowest > node.F) // Maybe this should be greater than not less than.
{
currentLowest = node.F;
currentNode = node;
}
}
return(currentNode);
}
public Structs.Node researchNode(int X, int Y, int endX, int endY, int i, int[,] mapMatrix, int pId)
{
Structs.Node currentTile = new Structs.Node();
currentTile.ID = i;
currentTile.X = X;
currentTile.Y = Y;
currentTile.speed = mapMatrix[X, Y];
currentTile.heuristic = getHeuristic(X, Y, endX, endY);
currentTile.parentID = pId;
if (mapMatrix[X, Y] == 3)
currentTile.walkable = false;
else
currentTile.walkable = true;
return currentTile;
}
public List <Structs.Node> ReconstructPath(List <Structs.Node> ClosedList, Structs.Node End, Structs.Node Start)
{
List <Structs.Node> Path = new List <Structs.Node>();
foreach (Structs.Node nd in ClosedList)
{
if (nd.X == End.X && nd.Y == End.Y)
{
Path.Add(nd); // First path point added.
}
}
while (Path[Path.Count - 1].X != Start.X || Path[Path.Count - 1].Y != Start.Y)
{
// Add last items parent to Path list (Path[Path.Count - 1].ParentID)
Path.Add(getParent(Path[Path.Count - 1].parentID, ClosedList));
}
return(Path);
}
public Structs.Node researchNode(int X, int Y, int endX, int endY, int i, int[,] mapMatrix, int pId)
{
Structs.Node currentTile = new Structs.Node();
currentTile.ID = i;
currentTile.X = X;
currentTile.Y = Y;
currentTile.speed = mapMatrix[X, Y];
currentTile.heuristic = getHeuristic(X, Y, endX, endY);
currentTile.parentID = pId;
if (mapMatrix[X, Y] == 3)
{
currentTile.walkable = false;
}
else
{
currentTile.walkable = true;
}
return(currentTile);
}
private void button4_Click(object sender, EventArgs e)
{
Structs.Node Start = new Structs.Node();
Start.X = startX;
Start.Y = startY;
Structs.Node End = new Structs.Node();
End.X = endX;
End.Y = endY;
List <Structs.Node> ClosedList = pathFinder.FindPath(Start, End, _mapMatrixPF);
List <Structs.Node> Path = pathFinder.ReconstructPath(ClosedList, End, Start);
pathList.Items.Add("Path:");
foreach (Structs.Node nd in Path)
{
_mapMatrix[nd.X, nd.Y] = 5;
this.Refresh();
pathList.Items.Add(Convert.ToString(nd.ID) + ". Pos: " + Convert.ToString(nd.X) + ", " + Convert.ToString(nd.Y) + ". F: " + Convert.ToString(nd.F) + ". PID: " + Convert.ToString(nd.parentID));
}
pathList.Items.Add("OpenList:");
foreach (Structs.Node nd in ClosedList)
{
pathList.Items.Add(Convert.ToString(nd.ID) + ". Pos: " + Convert.ToString(nd.X) + ", " + Convert.ToString(nd.Y) + ". F: " + Convert.ToString(nd.F) + ". PID: " + Convert.ToString(nd.parentID));
}
}
private void button4_Click(object sender, EventArgs e)
{
Structs.Node Start = new Structs.Node();
Start.X = startX;
Start.Y = startY;
Structs.Node End = new Structs.Node();
End.X = endX;
End.Y = endY;
List<Structs.Node> ClosedList = pathFinder.FindPath(Start, End, _mapMatrixPF);
List<Structs.Node> Path = pathFinder.ReconstructPath(ClosedList, End, Start);
pathList.Items.Add("Path:");
foreach (Structs.Node nd in Path)
{
_mapMatrix[nd.X, nd.Y] = 5;
this.Refresh();
pathList.Items.Add(Convert.ToString(nd.ID) + ". Pos: " + Convert.ToString(nd.X) + ", " + Convert.ToString(nd.Y) + ". F: " + Convert.ToString(nd.F) + ". PID: " + Convert.ToString(nd.parentID));
}
pathList.Items.Add("OpenList:");
foreach (Structs.Node nd in ClosedList)
{
pathList.Items.Add(Convert.ToString(nd.ID) + ". Pos: " + Convert.ToString(nd.X) + ", " + Convert.ToString(nd.Y) + ". F: " + Convert.ToString(nd.F) + ". PID: " + Convert.ToString(nd.parentID));
}
}
public List <Structs.Node> FindPath(Structs.Node Start, Structs.Node End, int[,] MapMatrix)
{
// A counter to check progress when debugging:
int index = 0;
float hMult;
bool Finished = false;
// Declare Open and Closed sets, Current Node
List <Structs.Node> OpenSet = new List <Structs.Node>();
List <Structs.Node> ClosedSet = new List <Structs.Node>();
Structs.Node CurrentNode = Start;
int i = 0;
//Configure the current node X, Y, G, H, F, and ID
CurrentNode.X = Start.X;
CurrentNode.Y = Start.Y;
CurrentNode.G = MapMatrix[Start.X, Start.Y]; // The G cost of each tile is the speed of that tile, as it costs that value to move to the next one.
CurrentNode.heuristic = (Math.Abs(CurrentNode.X - End.X) + Math.Abs(CurrentNode.Y - End.Y)); // The heuristic of the starting tile will always be fixed
CurrentNode.F = CurrentNode.G + CurrentNode.heuristic;
CurrentNode.ID = i;
// Add the current node (Starting Node) to the OpenSet for consideration.
OpenSet.Add(CurrentNode);
// While the open set contains nodes
while (Finished == false)
{
i++;
// Set the current node to the value in the open set with the lowest F (H + G)
CurrentNode = GetLowestF(OpenSet);
// If the current tile is the goal, return reconstruct path method
if (CurrentNode.X == End.X && CurrentNode.Y == End.Y)
{
// TODO : Add ReconstructPath to this return method.
ClosedSet.Add(CurrentNode);
return(ClosedSet);
}
// Remove the current tile from the open set, but retain it in current tile var
OpenSet.Remove(CurrentNode);
// Add the current tile to the closed set, retaining it in the current tile var
ClosedSet.Add(CurrentNode);
// For each neighbor tile
for (int x = -1; x < 2; x++)
{
for (int y = -1; y < 2; y++)
{
if (CurrentNode.X + x < 0 || CurrentNode.Y + y < 0 || CurrentNode.X + x > 8 || CurrentNode.Y + y > 8)
{
break;
}
if ((x == 1 && y == 1) || (x == -1 && y == -1) || (x == 1 && y == -1) || (x == -1 && y == 1))
{
hMult = 1.4f;
}
else
{
hMult = 1f;
}
index++;
// Create a neighbor node for investigation
Structs.Node neighbor = new Structs.Node();
neighbor.X = CurrentNode.X + x;
neighbor.Y = CurrentNode.Y + y;
neighbor.ID = i;
i++;
if (MapMatrix[neighbor.X, neighbor.Y] == 3)
{
break;
}
// Set Heuristic to the estimated heuristic
neighbor.heuristic = hMult * (Math.Abs(neighbor.X - End.X) + Math.Abs(neighbor.Y - End.Y));
// Set G score of neighbor to current G score + cost of moving from current to neighbor
neighbor.G = CurrentNode.G + MapMatrix[neighbor.X, neighbor.Y];
// TODO Modify this to set the parent to the best one for the node
neighbor.parentID = CurrentNode.ID;
neighbor.G = CurrentNode.G + MapMatrix[neighbor.X, neighbor.Y];
neighbor.F = neighbor.G + neighbor.heuristic;
// If the neighbor is in the closed set or the G score is higher than the current G score, break
// If the neighbor is not in the open set or the neighbor G score is less than the current G score
if ((!OpenSet.Contains(neighbor) || neighbor.G >= CurrentNode.G) && (neighbor.G >= CurrentNode.G || !ClosedSet.Contains(neighbor)))
{
if (!OpenSet.Contains(neighbor))
{
OpenSet.Add(neighbor);
}
if (neighbor.X == End.X && neighbor.Y == End.Y)
{
ClosedSet.Add(neighbor);
Finished = true;
}
}
}
}
}
// Return failure if the open set is empty (break the while loop)
return(ClosedSet);
}
public List<Structs.Node> FindPath(Structs.Node Start, Structs.Node End, int[,] MapMatrix)
{
// A counter to check progress when debugging:
int index = 0;
float hMult;
bool Finished = false;
// Declare Open and Closed sets, Current Node
List<Structs.Node> OpenSet = new List<Structs.Node>();
List<Structs.Node> ClosedSet = new List<Structs.Node>();
Structs.Node CurrentNode = Start;
int i = 0;
//Configure the current node X, Y, G, H, F, and ID
CurrentNode.X = Start.X;
CurrentNode.Y = Start.Y;
CurrentNode.G = MapMatrix[Start.X, Start.Y]; // The G cost of each tile is the speed of that tile, as it costs that value to move to the next one.
CurrentNode.heuristic = (Math.Abs(CurrentNode.X - End.X) + Math.Abs(CurrentNode.Y - End.Y)); // The heuristic of the starting tile will always be fixed
CurrentNode.F = CurrentNode.G + CurrentNode.heuristic;
CurrentNode.ID = i;
// Add the current node (Starting Node) to the OpenSet for consideration.
OpenSet.Add(CurrentNode);
// While the open set contains nodes
while (Finished == false)
{
i++;
// Set the current node to the value in the open set with the lowest F (H + G)
CurrentNode = GetLowestF(OpenSet);
// If the current tile is the goal, return reconstruct path method
if (CurrentNode.X == End.X && CurrentNode.Y == End.Y)
{
// TODO : Add ReconstructPath to this return method.
ClosedSet.Add(CurrentNode);
return ClosedSet;
}
// Remove the current tile from the open set, but retain it in current tile var
OpenSet.Remove(CurrentNode);
// Add the current tile to the closed set, retaining it in the current tile var
ClosedSet.Add(CurrentNode);
// For each neighbor tile
for (int x = -1; x < 2; x++)
{
for (int y = -1; y < 2; y++)
{
if (CurrentNode.X + x < 0 || CurrentNode.Y + y < 0 || CurrentNode.X + x > 8 || CurrentNode.Y + y > 8 )
{
break;
}
if ((x == 1 && y == 1) || (x == -1 && y == -1) || (x == 1 && y == -1) || (x == -1 && y == 1))
hMult = 1.4f;
else
hMult = 1f;
index++;
// Create a neighbor node for investigation
Structs.Node neighbor = new Structs.Node();
neighbor.X = CurrentNode.X + x;
neighbor.Y = CurrentNode.Y + y;
neighbor.ID = i;
i++;
if (MapMatrix[neighbor.X, neighbor.Y] == 3)
{
break;
}
// Set Heuristic to the estimated heuristic
neighbor.heuristic = hMult * (Math.Abs(neighbor.X - End.X) + Math.Abs(neighbor.Y - End.Y));
// Set G score of neighbor to current G score + cost of moving from current to neighbor
neighbor.G = CurrentNode.G + MapMatrix[neighbor.X, neighbor.Y];
// TODO Modify this to set the parent to the best one for the node
neighbor.parentID = CurrentNode.ID;
neighbor.G = CurrentNode.G + MapMatrix[neighbor.X, neighbor.Y];
neighbor.F = neighbor.G + neighbor.heuristic;
// If the neighbor is in the closed set or the G score is higher than the current G score, break
// If the neighbor is not in the open set or the neighbor G score is less than the current G score
if ((!OpenSet.Contains(neighbor) || neighbor.G >= CurrentNode.G) && (neighbor.G >= CurrentNode.G || !ClosedSet.Contains(neighbor)))
{
if (!OpenSet.Contains(neighbor))
{
OpenSet.Add(neighbor);
}
if (neighbor.X == End.X && neighbor.Y == End.Y)
{
ClosedSet.Add(neighbor);
Finished = true;
}
}
}
}
}
// Return failure if the open set is empty (break the while loop)
return ClosedSet;
}
public List<Structs.Node> findPath(int startX, int startY, int endX, int endY, int[,] mapMatrix)
{
int i = 0;
bool pathFound = false;
int currentH = Math.Abs(startX - startY) + Math.Abs(endX - endY);
List<Structs.Node> path = new List<Structs.Node>();
Structs.Node currentTile = new Structs.Node();
currentTile = researchNode(startX, startY, endX, endY, i, mapMatrix, -1);
openList.Add(currentTile);
while (!pathFound)
{
float lowestH = 65535;
// When we don't have a path, find the lowest H value in our openList
foreach (Structs.Node nd in openList)
{
// If its walkable and has lowest H value, make it the next tile to be examined.
if (nd.heuristic < lowestH && nd.walkable)
{
currentTile = nd;
lowestH = currentTile.heuristic;
}
// Else if it's not walkable, remove from open and add to closed list.
else if (!nd.walkable)
{
openList.Remove(nd);
closedList.Add(nd);
}
}
// Now research the tile around the selected node.
// For each value where the X coord is within 1 sqm of the node, as is the Y coord
for (int x = -1; x < 2; x++)
{
for (int y = -1; y < 2; y++)
{
if (x == 0 && y == 0)
{
}
else
{
try
{
if (mapMatrix[currentTile.X + x, currentTile.Y + y] == 2)
{
pathFound = true;
}
// Add the researched tile to the open list.
openList.Add(researchNode(currentTile.X + x, currentTile.Y + y, endX, endY, i, mapMatrix, currentTile.ID));
}
catch (Exception ex)
{
}
}
}
}
openList.Remove(currentTile);
}
return openList;
}