implement direction cost

This commit is contained in:
BOTLester 2020-05-03 23:40:40 +02:00
parent 1015886a71
commit 8433934bcd
2 changed files with 34 additions and 6 deletions

View File

@ -35,12 +35,13 @@ class Astar
var adjacentNodes = new List<Nodes>()
{
new Nodes(new Vector2(currentPos.X, currentPos.Y+1)),
new Nodes(new Vector2(currentPos.X + 1, currentPos.Y)),
new Nodes(new Vector2(currentPos.X, currentPos.Y - 1)),
new Nodes(new Vector2(currentPos.X - 1, currentPos.Y)),
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)
@ -56,12 +57,23 @@ class Astar
item => crops[(int)item.getCords().X, (int)item.getCords().Y].Status != 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));
}
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;
}
public Path FindPath()
{
Path path = new Path();
@ -72,6 +84,7 @@ class Astar
Nodes target = new Nodes(targetPos);
Nodes startPos = new Nodes (tractorPos);
Nodes current = null;
int direction = 6;
int g = 0;
//openList.Enqueue(startPos);
@ -87,6 +100,7 @@ class Astar
//closedList.Enqueue(current);
openList.removeMin();
//openList.Dequeue();
direction = current.getDirection();
if (current.getCords() == target.getCords())
break;
@ -99,9 +113,9 @@ class Astar
//g++;
foreach(var adjacentNode in adjacentNodes)
{
g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement();
if (closedList.Exists(adjacentNode.getCords()))
continue;
g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement() + CalculateRotationCost(direction, adjacentNode.getDirection());
if (!(openList.Exists(adjacentNode.getCords())))
{
@ -121,7 +135,9 @@ class Astar
adjacentNode.setParent(current);
}
}
}
//openList.BuildMinHeap(openList.GetList());
}

View File

@ -14,12 +14,19 @@ class Nodes
private int H = 0;
private Vector2 Coordinates;
private Nodes Parent = null;
private int Direction = 0;
public Nodes(Vector2 coordinates)
{
Coordinates = coordinates;
}
public Nodes(Vector2 coordinates, int direction)
{
Coordinates = coordinates;
Direction = direction;
}
public Nodes (Nodes node)
{
F = node.F;
@ -82,4 +89,9 @@ class Nodes
{
Parent = parent;
}
public int getDirection()
{
return Direction;
}
}