public void Run()
{
StartNode = m_Pathfind.FindPath(Start, End);
Done.Release();
}
public Node FindPath(Point Start, Point End)
{
Node[,] Nodes = new Node[GridWidth, GridHeight];
LinkedList<Node> OpenNodes = new LinkedList<Node>();
LinkedList<Node> ClosedNodes = new LinkedList<Node>();
int i, j;
// initialize end node, and add it to open list
OpenNodes.AddLast(AddNode(End, Start, ref Nodes));
// the heart of the A* algorithm
// I implemented this from memory, so it looks a little different from the professor's pseudocode
while (OpenNodes.Count > 0)
{
// pop first node from open list
Node CurrentNode = OpenNodes.First.Value;
OpenNodes.RemoveFirst();
ClosedNodes.AddLast(CurrentNode);
// if reached start node then exit
if (Nodes[Start.X, Start.Y] != null)
{
break;
}
// calculate cost to adjacent nodes, if less than current cost then add them to open list sorted by estimated total cost
foreach (Point AdjacentNodePos in CurrentNode.AdjacentNodes)
{
if (Nodes[AdjacentNodePos.X, AdjacentNodePos.Y] == null)
{
AddNode(AdjacentNodePos, Start, ref Nodes); // if node here hasn't been made yet, make it now
}
Node AdjacentNode = Nodes[AdjacentNodePos.X, AdjacentNodePos.Y];
float NewCostSoFar = CurrentNode.CostSoFar + Vector2.Distance(AdjacentNode.GridPos, CurrentNode.GridPos);
if ((AdjacentNode.Parent == null || NewCostSoFar < AdjacentNode.CostSoFar)
&& (AdjacentNode.GridPos.X != End.X || AdjacentNode.GridPos.Y != End.Y))
{
AdjacentNode.Parent = CurrentNode;
AdjacentNode.CostSoFar = NewCostSoFar;
for (LinkedListNode<Node> node = OpenNodes.First; ; node = node.Next)
{
if (node == null)
{
OpenNodes.AddLast(AdjacentNode); // AdjacentNode has greatest estimated total cost, add to end of list
break;
}
if (AdjacentNode == node.Value)
{
break; // AdjacentNode is already in open list, so don't add a duplicate
}
if (AdjacentNode.EstTotalCost() <= node.Value.EstTotalCost())
{
OpenNodes.AddBefore(node, AdjacentNode);
break;
}
}
}
}
}
if (Nodes[Start.X, Start.Y] != null)
{
// do path smoothing :)
for (Node CurrentNode = Nodes[Start.X, Start.Y]; CurrentNode != null; CurrentNode = CurrentNode.Parent)
{
bool Blocked = false;
for (Node NextNode = CurrentNode.Parent; NextNode != null && !Blocked; NextNode = NextNode.Parent)
{
Vector2 Parallel = NextNode.GridPos - CurrentNode.GridPos;
Vector2 Perpendicular = new Vector2(-Parallel.Y, Parallel.X);
Parallel.Normalize();
Perpendicular.Normalize();
for (i = 0; i < GridWidth && !Blocked; i++)
{
for (j = 0; j < GridHeight && !Blocked; j++)
{
// if distance from smoothed path to occupied square is less than 1 square then it is blocked
if (Occupied[i, j] && Math.Abs(Vector2.Dot(new Vector2(i, j) - CurrentNode.GridPos, Perpendicular)) < 1
&& Vector2.Dot(new Vector2(i, j) - CurrentNode.GridPos, Parallel) >= 0
&& Vector2.Dot(new Vector2(i, j) - NextNode.GridPos, Parallel) <= 0)
{
Blocked = true;
}
}
}
// if smoothed path is not blocked then update parent of current node
if (!Blocked)
{
CurrentNode.Parent = NextNode;
}
}
}
return Nodes[Start.X, Start.Y];
}
return null;
}
private Node AddNode(Point Pos, Point Start, ref Node[,] Nodes)
{
Nodes[Pos.X, Pos.Y] = new Node(new Vector2(Pos.X, Pos.Y), Vector2.Distance(new Vector2(Start.X, Start.Y), new Vector2(Pos.X, Pos.Y)));
// find adjacent nodes
bool left = false, right = false, above = false, below = false;
if (Pos.X > 0 && !Occupied[Pos.X - 1, Pos.Y])
{
Nodes[Pos.X, Pos.Y].AdjacentNodes.Add(new Point(Pos.X - 1, Pos.Y));
left = true;
}
if (Pos.X < GridWidth - 1 && !Occupied[Pos.X + 1, Pos.Y])
{
Nodes[Pos.X, Pos.Y].AdjacentNodes.Add(new Point(Pos.X + 1, Pos.Y));
right = true;
}
if (Pos.Y > 0 && !Occupied[Pos.X, Pos.Y - 1])
{
Nodes[Pos.X, Pos.Y].AdjacentNodes.Add(new Point(Pos.X, Pos.Y - 1));
above = true;
}
if (Pos.Y < GridHeight - 1 && !Occupied[Pos.X, Pos.Y + 1])
{
Nodes[Pos.X, Pos.Y].AdjacentNodes.Add(new Point(Pos.X, Pos.Y + 1));
below = true;
}
if (left && above && !Occupied[Pos.X - 1, Pos.Y - 1])
{
Nodes[Pos.X, Pos.Y].AdjacentNodes.Add(new Point(Pos.X - 1, Pos.Y - 1));
}
if (right && above && !Occupied[Pos.X + 1, Pos.Y - 1])
{
Nodes[Pos.X, Pos.Y].AdjacentNodes.Add(new Point(Pos.X + 1, Pos.Y - 1));
}
if (left && below && !Occupied[Pos.X - 1, Pos.Y + 1])
{
Nodes[Pos.X, Pos.Y].AdjacentNodes.Add(new Point(Pos.X - 1, Pos.Y + 1));
}
if (right && below && !Occupied[Pos.X + 1, Pos.Y + 1])
{
Nodes[Pos.X, Pos.Y].AdjacentNodes.Add(new Point(Pos.X + 1, Pos.Y + 1));
}
return Nodes[Pos.X, Pos.Y];
}
