forked from s425077/PotatoPlan
166 lines
6.0 KiB
C#
166 lines
6.0 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using Microsoft.Xna.Framework;
|
|
using C5;
|
|
using System.Diagnostics;
|
|
|
|
class Astar
|
|
{
|
|
|
|
private Vector2 tractorPos;
|
|
private Vector2 housePos;
|
|
private static Crops[,] crops;
|
|
private Vector2 Size;
|
|
private Vector2 targetPos;
|
|
private int Rotation;
|
|
|
|
public void update(Crops[,] newCrops, Vector2 newSize, Vector2 newTractorPos, Vector2 newHousePos, int rotation)
|
|
{
|
|
tractorPos = new Vector2((int)newTractorPos.X, (int)newTractorPos.Y);
|
|
housePos = new Vector2((int)newHousePos.X, (int)newHousePos.Y);
|
|
crops = newCrops;
|
|
Size = newSize;
|
|
Rotation = rotation;
|
|
}
|
|
|
|
public void update(Crops[,] newCrops, Vector2 newSize, Vector2 newTractorPos, Vector2 newHousePos, Vector2 newTargetPos, int rotation)
|
|
{
|
|
tractorPos = new Vector2((int)newTractorPos.X, (int)newTractorPos.Y);
|
|
housePos = new Vector2((int)newHousePos.X, (int)newHousePos.Y);
|
|
crops = newCrops;
|
|
Size = newSize;
|
|
Rotation = rotation;
|
|
targetPos = newTargetPos;
|
|
}
|
|
|
|
// Get all adjacent nodes
|
|
public List<Nodes> GetAdjacentNodes(Vector2 currentPos)
|
|
{
|
|
var adjacentNodes = new List<Nodes>()
|
|
|
|
{
|
|
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),
|
|
};
|
|
|
|
//check if out of range
|
|
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 - 1 || adjacentNodes[i].getCords().Y > Size.Y - 1)
|
|
adjacentNodes.Remove(adjacentNodes[i]);
|
|
}
|
|
}
|
|
// return if not an obstacle
|
|
return adjacentNodes.Where(
|
|
item => (crops[(int)item.getCords().X, (int)item.getCords().Y].getStatus()) != 0).ToList();
|
|
}
|
|
|
|
// Heuristic function, Manhattan method.
|
|
public int ComputeHScore(Vector2 currentNode, Vector2 endNote)
|
|
{
|
|
return (int)(Math.Abs(endNote.X - currentNode.X) + Math.Abs(endNote.Y - currentNode.Y));
|
|
}
|
|
// Rotation Cost
|
|
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 20;
|
|
else if (Math.Abs(currDir - newDir) == 0 || Math.Abs(currDir - newDir) == 2)
|
|
return 900;
|
|
return 0;
|
|
}
|
|
|
|
// Convert rotation used by sprite, to get direction of first node in next path
|
|
public int ConvertRotation()
|
|
{
|
|
int rotation = 0;
|
|
if (Rotation > 135 && Rotation < 225)
|
|
rotation = 0;
|
|
else if (Rotation > 225 && Rotation < 315)
|
|
rotation = 1;
|
|
else if (Rotation > 315 && Rotation < 45)
|
|
rotation = 2;
|
|
else if (Rotation > 45 && Rotation < 135)
|
|
rotation = -1;
|
|
return rotation;
|
|
}
|
|
|
|
// Main function of A* algorithm
|
|
public Path FindPath(bool flipArray)
|
|
{
|
|
int g = 0;
|
|
int direction = ConvertRotation();
|
|
Path path = new Path();
|
|
MinHeap openList = new MinHeap();
|
|
MinHeap closedList = new MinHeap();
|
|
Nodes target = new Nodes(targetPos);
|
|
Nodes startPos = new Nodes(tractorPos, direction);
|
|
Nodes current = null;
|
|
|
|
openList.Insert(startPos);
|
|
|
|
while (openList.GetSize() > 0)
|
|
{
|
|
current = openList.getMin();
|
|
closedList.Insert(current);
|
|
openList.removeMin();
|
|
direction = current.getDirection();
|
|
|
|
if (current.getCords() == target.getCords())
|
|
break;
|
|
|
|
var adjacentNodes = GetAdjacentNodes(current.getCords());
|
|
foreach (var adjacentNode in adjacentNodes)
|
|
{
|
|
if (closedList.Exists(adjacentNode.getCords())) // check if adjacent node is on closed list, if it is, skip it
|
|
continue;
|
|
g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement() + CalculateRotationCost(direction, adjacentNode.getDirection()); // calculate g - cost from start point
|
|
if (!(openList.Exists(adjacentNode.getCords()))) // if adjacent node is not on open list, add it
|
|
{
|
|
adjacentNode.setG(g);
|
|
adjacentNode.setH(ComputeHScore(adjacentNode.getCords(), target.getCords()));
|
|
adjacentNode.calculateF();
|
|
adjacentNode.setParent(current);
|
|
openList.Insert(adjacentNode);
|
|
}
|
|
else
|
|
{
|
|
if (g + adjacentNode.getH() < adjacentNode.getF()) // check if adjacent node is a better path than the current one
|
|
{
|
|
adjacentNode.setG(g);
|
|
adjacentNode.calculateF();
|
|
adjacentNode.setParent(current);
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
// backtrack to create path
|
|
while (current != null)
|
|
{
|
|
path.AddNode(current);
|
|
current = current.getParent();
|
|
}
|
|
|
|
if (flipArray)
|
|
path = path.FlipArray();
|
|
|
|
openList.deleteHeap();
|
|
closedList.deleteHeap();
|
|
|
|
return path;
|
|
}
|
|
|
|
} |