1
0
forked from s425077/PotatoPlan
PotatoPlan/Game1/Sources/Pathing/A-Star/Astar.cs
2020-05-03 17:52:35 +02:00

122 lines
3.6 KiB
C#

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;
private Vector2 targetPos;
public void update(Crops[,] newCrops, Vector2 newSize, Vector2 newTractorPos, Vector2 newHousePos, Vector2 newtargetPos)
{
tractorPos = new Vector2((int)newTractorPos.X, (int)newTractorPos.Y);
housePos = new Vector2((int)newHousePos.X, (int)newHousePos.Y);
targetPos = newtargetPos;
crops = newCrops;
Size = newSize;
}
public Nodes getOptimalPath()
{
return allPaths.Peek();
}
private List <Nodes> GetAdjacentNodes(Vector2 currentPos)
{
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)),
};
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]);
}
}
return adjacentNodes.Where(
item => crops[(int)item.getCords().X, (int)item.getCords().Y].Status != 0).ToList();
}
public int ComputeHScore(Vector2 currentNode, Vector2 endNote)
{
return (int)(Math.Abs(endNote.X - currentNode.X) + Math.Abs(endNote.Y - currentNode.Y));
}
public Path FindPath()
{
Path path = new Path();
PriorityQueue openList = new PriorityQueue();
PriorityQueue closedList = new PriorityQueue();
Nodes target = new Nodes(targetPos);
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() == target.getCords())
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(), target.getCords()));
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 != null)
{
path.AddNode(current);
current = current.getParent();
}
path.FlipArray();
return path;
}
}