1
0
forked from s425077/PotatoPlan
PotatoPlan/Game1/Sources/Pathing/A-Star/Astar.cs

157 lines
5.1 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>()
{
2020-05-03 23:40:40 +02:00
new Nodes(new Vector2(currentPos.X, currentPos.Y+1), 0),
new Nodes(new Vector2(currentPos.X + 1, currentPos.Y), 1),
new Nodes(new Vector2(currentPos.X, currentPos.Y - 1), 2),
new Nodes(new Vector2(currentPos.X - 1, currentPos.Y), -1),
2020-05-03 16:35:46 +02:00
};
2020-05-03 23:40:40 +02:00
//check if out of range
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
{
2020-05-03 19:30:41 +02:00
if (adjacentNodes[i].getCords().X > Size.X - 1 || adjacentNodes[i].getCords().Y > Size.Y - 1)
2020-05-03 17:06:03 +02:00
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 23:40:40 +02:00
// Heuristic function, Manhattan method.
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));
}
2020-05-03 23:40:40 +02:00
public int CalculateRotationCost(int currDir, int newDir)
{
if (currDir == newDir)
return 0;
else if (Math.Abs(currDir - newDir) == 1 || Math.Abs(currDir - newDir) == 3)
return 1;
else if (Math.Abs(currDir - newDir) == 0 || Math.Abs(currDir - newDir) == 2) //its for turning back, so it never should happen anyway, remove this check then?
return 10;
return 0;
}
2020-05-03 16:35:46 +02:00
public Path FindPath()
{
2020-05-03 17:06:03 +02:00
Path path = new Path();
2020-05-03 22:28:52 +02:00
//PriorityQueue openList = new PriorityQueue();
MinHeap openList = new MinHeap();
//PriorityQueue closedList = new PriorityQueue();
MinHeap closedList = new MinHeap();
2020-05-03 17:52:35 +02:00
Nodes target = new Nodes(targetPos);
2020-05-03 16:35:46 +02:00
Nodes startPos = new Nodes (tractorPos);
Nodes current = null;
2020-05-03 23:40:40 +02:00
int direction = 6;
2020-05-03 16:35:46 +02:00
int g = 0;
2020-05-03 22:28:52 +02:00
//openList.Enqueue(startPos);
openList.Insert(startPos);
2020-05-03 16:35:46 +02:00
2020-05-03 22:28:52 +02:00
//while (openList.Count > 0)
while (openList.GetSize() > 0)
2020-05-03 16:35:46 +02:00
{
2020-05-03 22:28:52 +02:00
current = openList.getMin();
//current = openList.Peek();
2020-05-03 19:30:41 +02:00
//g = current.getG();
2020-05-03 22:28:52 +02:00
closedList.Insert(current);
//closedList.Enqueue(current);
openList.removeMin();
//openList.Dequeue();
2020-05-03 23:40:40 +02:00
direction = current.getDirection();
2020-05-03 16:35:46 +02:00
2020-05-03 22:28:52 +02:00
if (current.getCords() == target.getCords())
2020-05-03 19:30:41 +02:00
break;
2020-05-03 16:35:46 +02:00
2020-05-03 22:28:52 +02:00
//if (closedList.Exists(target.getCords()))
// break;
2020-05-03 16:35:46 +02:00
var adjacentNodes = GetAdjacentNodes(current.getCords());
2020-05-03 19:30:41 +02:00
//g++;
2020-05-03 16:35:46 +02:00
foreach(var adjacentNode in adjacentNodes)
{
if (closedList.Exists(adjacentNode.getCords()))
continue;
2020-05-03 23:40:40 +02:00
g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement() + CalculateRotationCost(direction, adjacentNode.getDirection());
2020-05-03 16:35:46 +02:00
if (!(openList.Exists(adjacentNode.getCords())))
{
2020-05-03 19:30:41 +02:00
2020-05-03 16:35:46 +02:00
adjacentNode.setG(g);
2020-05-03 17:52:35 +02:00
adjacentNode.setH(ComputeHScore(adjacentNode.getCords(), target.getCords()));
2020-05-03 16:35:46 +02:00
adjacentNode.calculateF();
adjacentNode.setParent(current);
2020-05-03 22:28:52 +02:00
openList.Insert(adjacentNode);
//openList.Enqueue(adjacentNode);
2020-05-03 16:35:46 +02:00
}
else
{
if(g + adjacentNode.getH() < adjacentNode.getF())
{
adjacentNode.setG(g);
adjacentNode.calculateF();
adjacentNode.setParent(current);
}
}
2020-05-03 23:40:40 +02:00
2020-05-03 16:35:46 +02:00
}
2020-05-03 23:40:40 +02:00
2020-05-03 22:28:52 +02:00
//openList.BuildMinHeap(openList.GetList());
2020-05-03 16:35:46 +02:00
}
2020-05-03 17:52:35 +02:00
while (current != null)
2020-05-03 16:35:46 +02:00
{
path.AddNode(current);
current = current.getParent();
}
2020-05-03 19:30:41 +02:00
path = path.FlipArray();
2020-05-03 22:28:52 +02:00
openList.deleteHeap();
closedList.deleteHeap();
//openList.Clear();
//closedList.Clear();
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
}