PotatoPlan/route-planning.md

# intelligent tractor route planning implementation report

A* has been implemented. As of now, there are 4 types of fields wtih different costs:
	Grass 	- 1
	Dirt 	- 7
	Crops	- 15
	4th type are obstacles (mountains) which are unpassable.
	
Apart from costs of each fields also cost or turning the agent has been implemented.
	Turning once costs 2
	Turning twice costs 9
Cost of turning twice is so high to encourage the agent to try different path rather that just turning back after reaching its destination point.

All costs can be changed and are not final values.

Very rarely agent will make a little bump instead of going forward.
As of now we were not able to detect if it is a problem with A* algorithm or some other part of the program.

Snippets of code:

A* main loop:

        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();
        }


Successor function:

 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].Status != 0).ToList();
    }


Heuristic function:

   // 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));
    }