A* implemented and working, Task2 Ready

This commit is contained in:
BOTLester 2020-05-04 01:50:43 +02:00
parent 5af50f8d3b
commit 73b078ec7a
5 changed files with 48 additions and 112 deletions

View File

@ -31,15 +31,15 @@ class Crops
public int getCostOnMovement() public int getCostOnMovement()
{ {
if (Status == 1) if (Status == 1) //grass
{ {
return 1; return 1;
} }
else if (Status == 2) else if (Status == 2) //dirt
{ {
return 5; return 8;
} }
else if (Status == 3) else if (Status == 3) //crops
{ {
return 15; return 15;
} }

View File

@ -31,6 +31,7 @@ class Astar
return allPaths.Peek(); return allPaths.Peek();
} }
// Get all adjacent nodes
private List<Nodes> GetAdjacentNodes(Vector2 currentPos) private List<Nodes> GetAdjacentNodes(Vector2 currentPos)
{ {
var adjacentNodes = new List<Nodes>() var adjacentNodes = new List<Nodes>()
@ -53,7 +54,7 @@ class Astar
adjacentNodes.Remove(adjacentNodes[i]); adjacentNodes.Remove(adjacentNodes[i]);
} }
} }
// return if not an obstacle
return adjacentNodes.Where( return adjacentNodes.Where(
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();
} }
@ -63,18 +64,19 @@ class Astar
{ {
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));
} }
// Rotation Cost
public int CalculateRotationCost(int currDir, int newDir) public int CalculateRotationCost(int currDir, int newDir)
{ {
if (currDir == newDir) if (currDir == newDir)
return 0; return 0;
else if (Math.Abs(currDir - newDir) == 1 || Math.Abs(currDir - newDir) == 3) else if (Math.Abs(currDir - newDir) == 1 || Math.Abs(currDir - newDir) == 3)
return 3; return 2;
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? else if (Math.Abs(currDir - newDir) == 0 || Math.Abs(currDir - newDir) == 2)
return 9; return 9;
return 0; return 0;
} }
// Convert rotation used by sprite, to get direction of first node in next path
public int ConvertRotation() public int ConvertRotation()
{ {
int rotation = 0; int rotation = 0;
@ -89,50 +91,37 @@ class Astar
return rotation; return rotation;
} }
// Main function of A* algorithm
public Path FindPath() public Path FindPath()
{ {
int g = 0;
int direction = ConvertRotation();
Path path = new Path(); Path path = new Path();
//PriorityQueue openList = new PriorityQueue();
MinHeap openList = new MinHeap(); MinHeap openList = new MinHeap();
//PriorityQueue closedList = new PriorityQueue();
MinHeap closedList = new MinHeap(); MinHeap closedList = new MinHeap();
Nodes target = new Nodes(targetPos); Nodes target = new Nodes(targetPos);
int direction = ConvertRotation();
Nodes startPos = new Nodes(tractorPos, direction); Nodes startPos = new Nodes(tractorPos, direction);
Nodes current = null; Nodes current = null;
int g = 0;
//int direction = SmartTractor.update
//openList.Enqueue(startPos);
openList.Insert(startPos); openList.Insert(startPos);
//while (openList.Count > 0)
while (openList.GetSize() > 0) while (openList.GetSize() > 0)
{ {
current = openList.getMin(); current = openList.getMin();
//current = openList.Peek();
//g = current.getG();
closedList.Insert(current); closedList.Insert(current);
//closedList.Enqueue(current);
openList.removeMin(); openList.removeMin();
//openList.Dequeue();
direction = current.getDirection(); direction = current.getDirection();
if (current.getCords() == target.getCords()) if (current.getCords() == target.getCords())
break; break;
//if (closedList.Exists(target.getCords()))
// break;
var adjacentNodes = GetAdjacentNodes(current.getCords()); var adjacentNodes = GetAdjacentNodes(current.getCords());
//g++;
foreach (var adjacentNode in adjacentNodes) foreach (var adjacentNode in adjacentNodes)
{ {
if (closedList.Exists(adjacentNode.getCords())) if (closedList.Exists(adjacentNode.getCords())) // check if adjacent node is on closed list, if it is, skip it
continue; continue;
g = current.getG() + crops[(int)adjacentNode.getCords().X, (int)adjacentNode.getCords().Y].getCostOnMovement() + CalculateRotationCost(direction, adjacentNode.getDirection()); 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 (!(openList.Exists(adjacentNode.getCords()))) // if adjacent node is not on open list, add it
{ {
adjacentNode.setG(g); adjacentNode.setG(g);
@ -140,11 +129,10 @@ class Astar
adjacentNode.calculateF(); adjacentNode.calculateF();
adjacentNode.setParent(current); adjacentNode.setParent(current);
openList.Insert(adjacentNode); openList.Insert(adjacentNode);
//openList.Enqueue(adjacentNode);
} }
else else
{ {
if (g + adjacentNode.getH() < adjacentNode.getF()) if (g + adjacentNode.getH() < adjacentNode.getF()) // check if adjacent node is a better path than the current one
{ {
adjacentNode.setG(g); adjacentNode.setG(g);
adjacentNode.calculateF(); adjacentNode.calculateF();
@ -153,20 +141,18 @@ class Astar
} }
} }
//openList.BuildMinHeap(openList.GetList());
} }
// backtrack to create path
while (current != null) while (current != null)
{ {
path.AddNode(current); path.AddNode(current);
current = current.getParent(); current = current.getParent();
} }
path = path.FlipArray(); path = path.FlipArray();
openList.deleteHeap(); openList.deleteHeap();
closedList.deleteHeap(); closedList.deleteHeap();
//openList.Clear();
//closedList.Clear();
return path; return path;
} }

View File

@ -121,8 +121,6 @@ class MinHeap
if (arr.Count > 0) if (arr.Count > 0)
{ {
//clear the current heap //clear the current heap
//Array.Resize(ref arr, input.Length);
//heapSize = 0;
for (int i = 0; i < arr.Count; i++) for (int i = 0; i < arr.Count; i++)
{ {
arr[i] = input[i]; arr[i] = input[i];

View File

@ -61,80 +61,4 @@ class Path
return nodes[i]; return nodes[i];
} }
/*
private int Count = 0;
private int Cost = 0;
private int Efficency;
public Nodes getNode(int i)
{
return nodes[i];
}
public void setNode(Vector2 newNode)
{
nodes[Count] = new Nodes(10, newNode);
Count++;
}
public Nodes Reduce()
{
Count--;
Nodes temp = nodes[0];
for (int i = 0; i < Count; i++)
{
nodes[i] = nodes[i + 1];
}
return temp;
}
public Nodes getFinalDest()
{
return nodes[Count];
}
public int getCount()
{
return Count;
}
public Nodes getFirst()
{
return nodes[0];
}
public Nodes getByIndex(int i)
{
return nodes[i];
}
public int getEfficency()
{
return Efficency;
}
private void calculateEfficency()
{
for (int i = 0; i < Count; i++)
{
Efficency = Efficency + nodes[i].getCost();
}
}
public int getCost()
{
return Cost;
}
public void setCost(Crops Crop)
{
}
*/
} }

28
route-planning.md Normal file
View File

@ -0,0 +1,28 @@
# intelligent tractor environment implementation report
The environment is a resizable two dimensional grid that has sprites for a farmhouse, crops and empty fields
House:
![](https://lh4.googleusercontent.com/tEGhmLT2PAgvXG313YaVewDrUhMYgvY1PPmPpISbcW2BRz4UPfDph3-86YDHjHl4YdyCYAx4qhzB2fTlmA2Gh5nyg6oLM92Nwg-UtV3tKw3nLBe3RZvr47efqYMElmaQASTr-xMt)
The tractor (in white) and crop (in red) on the field:
![](https://lh4.googleusercontent.com/vd2n3rGPXDMLaaGBGBtjxVRftNryut-NYPstA4_CJwtRDrIVKVoAt-YrSZhcTysnk4cPXNEtssbVx02O9-DHUlV_JkY7nudSqanG7vCnMVMXQHSDnVjhDG8q98OelzNfxNXsMrbH)
The simulation can increase in speed or even be halted with the arrow keys, the current speed is displayed at the bottom left of the window The current environment spawns crops in randomly generated locations and the tractor tracks the location of crops and it will haul the target back to the house as well as fertilize newly planted crops.
As a task is complete on a tile its stage level will increase by one all the way up to 4 depending when it is fully grown and can be harvested,
1. The soil will not support crops.
2. Soil is ready for crops to be planted.
3. Crops are planted and need fertilization.
4. Crops are ready for harvest.
After the crops have been harvested they will return to stage 2.
A short video of how the project works:[video](https://uam-my.sharepoint.com/:v:/g/personal/domhof1_st_amu_edu_pl/Ec1ThqI2DSZNob2-6IrfPvYBe9_G2xfK8Ffn04dSklsWIQ?e=OrTqmy)