104 lines
4.2 KiB
Markdown
104 lines
4.2 KiB
Markdown
# 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 - 8
|
|
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:
|
|
|
|
public int ComputeHScore(Vector2 currentNode, Vector2 endNote)
|
|
{
|
|
return (int)(Math.Abs(endNote.X - currentNode.X) + Math.Abs(endNote.Y - currentNode.Y));
|
|
} |