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

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;
    private int 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);
        targetPos = newtargetPos;
        crops = newCrops;
        Size = newSize;
        Rotation = rotation;
    }

    public Nodes getOptimalPath()
    {
        return allPaths.Peek();
    }

    // Get all adjacent nodes
    private 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 2;
        else if (Math.Abs(currDir - newDir) == 0 || Math.Abs(currDir - newDir) == 2)
            return 9;
        return 0;
    }

    // Convert rotation used by sprite, to get direction of first node in next path
    public int ConvertRotation()
    {
        int rotation = 0;
        if (Rotation == 180)
            rotation = 0;
        else if (Rotation == 270)
            rotation = 1;
        else if (Rotation == 0)
            rotation = 2;
        else if (Rotation == 90)
            rotation = -1;
        return rotation;
    }

    // Main function of A* algorithm
    public Path FindPath()
    {
        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();
        }
        path = path.FlipArray();

        openList.deleteHeap();
        closedList.deleteHeap();
        return path;
    }

}