1
0
forked from s425077/PotatoPlan

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>() var adjacentNodes = new List<Nodes>()
{ {
new Nodes(new Vector2(currentPos.X, currentPos.Y+1)), new Nodes(new Vector2(currentPos.X, currentPos.Y+1), 0),
new Nodes(new Vector2(currentPos.X + 1, currentPos.Y)), new Nodes(new Vector2(currentPos.X + 1, currentPos.Y), 1),
new Nodes(new Vector2(currentPos.X, currentPos.Y - 1)), new Nodes(new Vector2(currentPos.X, currentPos.Y - 1), 2),
new Nodes(new Vector2(currentPos.X - 1, currentPos.Y)), new Nodes(new Vector2(currentPos.X - 1, currentPos.Y), -1),
}; };
//check if out of range
for (int i = 3; i >=0; i--) for (int i = 3; i >=0; i--)
{ {
if (adjacentNodes[i].getCords().X < 0 || adjacentNodes[i].getCords().Y < 0) 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(); item => crops[(int)item.getCords().X, (int)item.getCords().Y].Status != 0).ToList();
} }
// Heuristic function, Manhattan method.
public int ComputeHScore(Vector2 currentNode, Vector2 endNote) public int ComputeHScore(Vector2 currentNode, Vector2 endNote)
{ {
return (int)(Math.Abs(endNote.X - currentNode.X) + Math.Abs(endNote.Y - currentNode.Y)); 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() public Path FindPath()
{ {
Path path = new Path(); Path path = new Path();
@ -72,6 +84,7 @@ class Astar
Nodes target = new Nodes(targetPos); Nodes target = new Nodes(targetPos);
Nodes startPos = new Nodes (tractorPos); Nodes startPos = new Nodes (tractorPos);
Nodes current = null; Nodes current = null;
int direction = 6;
int g = 0; int g = 0;
//openList.Enqueue(startPos); //openList.Enqueue(startPos);
@ -87,6 +100,7 @@ class Astar
//closedList.Enqueue(current); //closedList.Enqueue(current);
openList.removeMin(); openList.removeMin();
//openList.Dequeue(); //openList.Dequeue();
direction = current.getDirection();
if (current.getCords() == target.getCords()) if (current.getCords() == target.getCords())
break; break;
@ -99,9 +113,9 @@ class Astar
//g++; //g++;
foreach(var adjacentNode in adjacentNodes) foreach(var adjacentNode in adjacentNodes)
{ {
g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement();
if (closedList.Exists(adjacentNode.getCords())) if (closedList.Exists(adjacentNode.getCords()))
continue; continue;
g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement() + CalculateRotationCost(direction, adjacentNode.getDirection());
if (!(openList.Exists(adjacentNode.getCords()))) if (!(openList.Exists(adjacentNode.getCords())))
{ {
@ -121,7 +135,9 @@ class Astar
adjacentNode.setParent(current); adjacentNode.setParent(current);
} }
} }
} }
//openList.BuildMinHeap(openList.GetList()); //openList.BuildMinHeap(openList.GetList());
} }

View File

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