PotatoPlan/Game1/Sources/Pathing/A-Star/Astar.cs

121 lines
3.6 KiB
C#
Raw Normal View History

2020-05-03 13:05:05 +02:00
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Xna.Framework;
class Astar
{
private Vector2 tractorPos;
private Vector2 housePos;
private Crops[,] crops;
private Vector2 Size;
private PriorityQueue allPaths;
2020-05-03 16:35:46 +02:00
private Vector2 targetPos;
2020-05-03 13:05:05 +02:00
2020-05-03 16:35:46 +02:00
public void update(Crops[,] newCrops, Vector2 newSize, Vector2 newTractorPos, Vector2 newHousePos, Vector2 newtargetPos)
2020-05-03 13:05:05 +02:00
{
tractorPos = new Vector2((int)newTractorPos.X, (int)newTractorPos.Y);
housePos = new Vector2((int)newHousePos.X, (int)newHousePos.Y);
2020-05-03 16:35:46 +02:00
targetPos = newtargetPos;
2020-05-03 13:05:05 +02:00
crops = newCrops;
Size = newSize;
}
2020-05-03 16:35:46 +02:00
public Nodes getOptimalPath()
2020-05-03 13:05:05 +02:00
{
2020-05-03 16:35:46 +02:00
return allPaths.Peek();
2020-05-03 13:05:05 +02:00
}
2020-05-03 16:35:46 +02:00
private List <Nodes> GetAdjacentNodes(Vector2 currentPos)
2020-05-03 13:05:05 +02:00
{
2020-05-03 16:35:46 +02:00
var adjacentNodes = new List<Nodes>()
{
new Nodes(new Vector2(currentPos.X, currentPos.Y+1)),
new Nodes(new Vector2(currentPos.X + 1, currentPos.Y)),
new Nodes(new Vector2(currentPos.X, currentPos.Y - 1)),
new Nodes(new Vector2(currentPos.X - 1, currentPos.Y)),
};
2020-05-03 17:06:03 +02:00
for (int i = 3; i >=0; i--)
{
if (adjacentNodes[i].getCords().X < 0 || adjacentNodes[i].getCords().Y < 0)
adjacentNodes.Remove(adjacentNodes[i]);
else
{
if (adjacentNodes[i].getCords().X > Size.X || adjacentNodes[i].getCords().Y > Size.Y)
adjacentNodes.Remove(adjacentNodes[i]);
}
}
2020-05-03 16:35:46 +02:00
return adjacentNodes.Where(
item => crops[(int)item.getCords().X, (int)item.getCords().Y].Status != 0).ToList();
}
2020-05-03 17:06:03 +02:00
2020-05-03 16:35:46 +02:00
public int ComputeHScore(Vector2 currentNode, Vector2 endNote)
{
return (int)(Math.Abs(endNote.X - currentNode.X) + Math.Abs(endNote.Y - currentNode.Y));
}
public Path FindPath()
{
2020-05-03 17:06:03 +02:00
Path path = new Path();
2020-05-03 16:35:46 +02:00
PriorityQueue openList = new PriorityQueue();
PriorityQueue closedList = new PriorityQueue();
Nodes startPos = new Nodes (tractorPos);
Nodes current = null;
int g = 0;
openList.Enqueue(startPos);
while (openList.Count > 0)
{
current = openList.Peek();
closedList.Enqueue(current);
openList.Dequeue();
if (closedList.Peek().getCords() == targetPos)
break;
var adjacentNodes = GetAdjacentNodes(current.getCords());
g++;
foreach(var adjacentNode in adjacentNodes)
{
if (closedList.Exists(adjacentNode.getCords()))
continue;
if (!(openList.Exists(adjacentNode.getCords())))
{
adjacentNode.setG(g);
adjacentNode.setH(ComputeHScore(adjacentNode.getCords(), targetPos));
adjacentNode.calculateF();
adjacentNode.setParent(current);
openList.Enqueue(adjacentNode);
}
else
{
if(g + adjacentNode.getH() < adjacentNode.getF())
{
adjacentNode.setG(g);
adjacentNode.calculateF();
adjacentNode.setParent(current);
}
}
}
}
while (current.getParent() != null)
{
path.AddNode(current);
current = current.getParent();
}
2020-05-03 17:06:03 +02:00
path.FlipArray();
2020-05-03 16:35:46 +02:00
return path;
2020-05-03 13:05:05 +02:00
}
2020-05-03 16:35:46 +02:00
}