//function that calculates all the intermediate points between each turning point (or jumpPoint if you may)
private void DrawPoints(GridLine x, int agvIndex)
{
//think of the incoming GridLine as follows:
//If you want to move from A to B, there might be an obstacle in the way, which must be bypassed
//For this purpose, there must be found a point to break the final route into 2 smaller (let's say A->b + b->B (AB in total)
//The incoming GridLine contains the pair of Coordinates for each one of the smaller routes
//So, for our example, GridLine x containts the starting A(x,y) & b(x,y)
//In a nutshell, this functions calculates all the child-steps of the parent-Line, determined by x.fromX,x.fromY and x.toX,x.toY
//the parent-Line will finaly consist of many pairs of (x,y): e.g [X1,Y1 / X2,Y2 / X3,Y3 ... Xn,Yn]
var x1 = x.FromX;
var y1 = x.FromY;
var x2 = x.ToX;
var y2 = x.ToY;
double distance = _f.GetLength(x1, y1, x2, y2); //function that returns the Euclidean distance between 2 points
double side = _f.getSide(_rectangles[0][0].Height
, _rectangles[0][0].Height); //function that returns the hypotenuse of a GridBox
int distanceBlocks = -1; //the quantity of blocks,matching the current line's length
//Calculates the number of GridBoxes that the Line consists of. Calculation depends on 2 scenarios:
//Scenario 1: Line is Diagonal
//Scenario 2: Line is Straight
if ((x1 < x2) && (y1 < y2)) //diagonal-right bottom direction
{
distanceBlocks = Convert.ToInt32(distance / side);
}
else if ((x1 < x2) && (y1 > y2)) //diagonal-right top direction
{
distanceBlocks = Convert.ToInt32(distance / side);
}
else if ((x1 > x2) && (y1 < y2)) //diagonal-left bottom direction
{
distanceBlocks = Convert.ToInt32(distance / side);
}
else if ((x1 > x2) && (y1 > y2)) //diagonal-left top direction
{
distanceBlocks = Convert.ToInt32(distance / side);
}
else if ((y1 == y2) || (x1 == x2)) //horizontal or vertical
{
distanceBlocks = Convert.ToInt32(distance / _rectangles[0][0].Width);
}
else
{
MessageBox.Show(this, "Unexpected error", "", MessageBoxButtons.OK, MessageBoxIcon.Error);
}
//1d array of points.used to track all the points of current line
Point[] currentLinePoints = new Point[distanceBlocks];
//here we calculate the X,Y coordinates of all the intermediate points
for (var i = 0; i < distanceBlocks; i++)
{
_calibrated = false;
double t;
if (distance != 0) //obviously, distance cannot be zero
{
t = ((side) / distance);
}
else
{
return;
}
//these are the x,y coord that are calculated in every for-loop
_a = Convert.ToInt32(((1 - t) * x1) + (t * x2));
_b = Convert.ToInt32(((1 - t) * y1) + (t * y2));
Point p = new Point(_a, _b); //merges the calculated x,y into 1 Point variable
for (var k = 0; k < Globals.WidthBlocks; k++)
{
for (var l = 0; l < Globals.HeightBlocks; l++)
{
if (_rectangles[k][l].BoxRec.Contains(p)) //this is how we assign the previously calculated pair of X,Y to a GridBox
//a smart way to handle GridBoxes from their center
{
int sideX = _rectangles[k][l].BoxRec.X + ((Globals.BlockSide / 2) - 1);
int sideY = _rectangles[k][l].BoxRec.Y + ((Globals.BlockSide / 2) - 1);
currentLinePoints[i].X = sideX;
currentLinePoints[i].Y = sideY;
if (dotsToolStripMenuItem.Checked)
{
using (SolidBrush br = new SolidBrush(Color.BlueViolet))
_paper.FillEllipse(br, currentLinePoints[i].X - 3,
currentLinePoints[i].Y - 3,
5, 5);
}
using (Font stepFont = new Font("Tahoma", 8, FontStyle.Bold))//Font used for numbering the steps/current block)
{
using (SolidBrush fontBr = new SolidBrush(Color.FromArgb(53, 153, 153)))
if (stepsToolStripMenuItem.Checked)
{
_paper.DrawString(_AGVs[agvIndex].StepsCounter + ""
, stepFont
, fontBr
, currentLinePoints[i]);
}
}
_calibrated = true;
}
}
}
if (_calibrated) //for each one of the above calculations, we check if the calibration has been done correctly and, if so, each pair is inserted to the corresponding AGV's steps List
{
_AGVs[agvIndex].Steps[_AGVs[agvIndex].StepsCounter].X = currentLinePoints[i].X;
_AGVs[agvIndex].Steps[_AGVs[agvIndex].StepsCounter].Y = currentLinePoints[i].Y;
_AGVs[agvIndex].StepsCounter++;
}
//initialize next steps.
x1 = currentLinePoints[i].X;
y1 = currentLinePoints[i].Y;
distance = _f.GetLength(x1, y1, x2, y2);
}
}
//Basic path planner function
private void Redraw()
{
bool startFound = false;
bool endFound = false;
_mapHasLoads = false;
GridPos endPos = new GridPos();
_posIndex = 0;
_startPos = new List <GridPos>(); //list that will be filled with the starting points of every AGV
_AGVs = new List <Vehicle>(); //list that will be filled with objects of the class Vehicle
_loadPos = new List <GridPos>(); //list that will be filled with the points of every Load
_loads = 0;
//Double FOR-loops to scan the whole Grid and perform the needed actions
for (var i = 0; i < Globals.WidthBlocks; i++)
{
for (var j = 0; j < Globals.HeightBlocks; j++)
{
if (_rectangles[i][j].BoxType == BoxType.Wall)
{
_searchGrid.SetWalkableAt(new GridPos(i, j), false);//Walls are marked as non-walkable
}
else
{
_searchGrid.SetWalkableAt(new GridPos(i, j), true);//every other block is marked as walkable (for now)
}
if (_rectangles[i][j].BoxType == BoxType.Load)
{
_mapHasLoads = true;
_searchGrid.SetWalkableAt(new GridPos(i, j), false); //marks every Load as non-walkable
_isLoad[i, j] = 1; //considers every Load as available
_loads++; //counts the number of available Loads in the grid
_loadPos.Add(new GridPos(i, j)); //inserts the coordinates of the Load inside a list
}
if (_rectangles[i][j].BoxType == BoxType.Normal)
{
_rectangles[i][j].OnHover(_boxDefaultColor);
}
if (_rectangles[i][j].BoxType == BoxType.Start)
{
if (_beforeStart)
{
_searchGrid.SetWalkableAt(new GridPos(i, j), false); //initial starting points of AGV are non walkable until 1st run is completed
}
else
{
_searchGrid.SetWalkableAt(new GridPos(i, j), true);
}
startFound = true;
_AGVs.Add(new Vehicle(this));
_AGVs[_posIndex].ID = _posIndex;
_startPos.Add(new GridPos(i, j)); //adds the starting coordinates of an AGV to the StartPos list
//a & b are used by DrawPoints() as the starting x,y for calculation purposes
_a = _startPos[_posIndex].X;
_b = _startPos[_posIndex].Y;
if (_posIndex < _startPos.Count)
{
_startPos[_posIndex] = new GridPos(_startPos[_posIndex].X, _startPos[_posIndex].Y);
_posIndex++;
}
}
if (_rectangles[i][j].BoxType == BoxType.End)
{
endFound = true;
endPos.X = i;
endPos.Y = j;
_endPointCoords = new Point(i * Globals.BlockSide, j * Globals.BlockSide + Globals.TopBarOffset);
}
}
}
if (!startFound || !endFound)
{
return; //will return if there are no starting or end points in the Grid
}
_posIndex = 0;
if (_AGVs != null)
{
for (short i = 0; i < _AGVs.Count(); i++)
{
if (_AGVs[i] != null)
{
_AGVs[i].UpdateAGV();
_AGVs[i].Status.Busy = false; //initialize the status of _AGVs, as 'available'
}
}
}
_startPos = NotTrappedVehicles(_startPos, endPos); //replaces the List with all the inserted _AGVs
//with a new one containing the right ones
if (_mapHasLoads)
{
KeepValidLoads(endPos); //calls a function that checks which Loads are available
}
//to be picked up by _AGVs and removed the trapped ones.
//For-loop to repeat the path-finding process for ALL the _AGVs that participate in the simulation
for (short i = 0; i < _startPos.Count; i++)
{
if (_loadPos.Count != 0)
{
var task = System.Threading.Tasks.Task.Run(() => CheckForTrappedLoads(_loadPos, endPos));
_loadPos = task.Result;
//_loadPos = await CheckForTrappedLoads(_loadPos, endPos);
}
if (_loadPos.Count == 0)
{
_mapHasLoads = false;
_AGVs[i].HasLoadToPick = false;
}
else
{
_mapHasLoads = true;
_AGVs[i].HasLoadToPick = true;
}
if (_AGVs[i].Status.Busy == false)
{
List <GridPos> jumpPointsList;
switch (_mapHasLoads)
{
case true:
//====create the path FROM START TO LOAD, if load exists=====
for (int m = 0; m < _loadPos.Count; m++)
{
_searchGrid.SetWalkableAt(_loadPos[m], false); //Do not allow walk over any other load except the targeted one
}
_searchGrid.SetWalkableAt(_loadPos[0], true);
//use of the A* alorithms to find the path between AGV and its marked Load
_jumpParam.Reset(_startPos[_posIndex], _loadPos[0]);
jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight);
_AGVs[i].JumpPoints = jumpPointsList;
_AGVs[i].Status.Busy = true;
//====create the path FROM START TO LOAD, if load exists=====
//======FROM LOAD TO END======
for (int m = 0; m < _loadPos.Count; m++)
{
_searchGrid.SetWalkableAt(_loadPos[m], false);
}
_jumpParam.Reset(_loadPos[0], endPos);
jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight);
_AGVs[i].JumpPoints.AddRange(jumpPointsList);
//marks the load that each AGV picks up on the 1st route, as 3, so each agv knows where to go after delivering the 1st load
_isLoad[_loadPos[0].X, _loadPos[0].Y] = 3;
_AGVs[i].MarkedLoad = new Point(_loadPos[0].X, _loadPos[0].Y);
_loadPos.Remove(_loadPos[0]);
//======FROM LOAD TO END======
break;
case false:
_jumpParam.Reset(_startPos[_posIndex], endPos);
jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight);
_AGVs[i].JumpPoints = jumpPointsList;
break;
}
}
_posIndex++;
}
int c = 0;
for (short i = 0; i < _startPos.Count; i++)
{
c += _AGVs[i].JumpPoints.Count;
}
for (short i = 0; i < _startPos.Count; i++)
{
for (int j = 0; j < _AGVs[i].JumpPoints.Count - 1; j++)
{
GridLine line = new GridLine
(
_rectangles[_AGVs[i].JumpPoints[j].X][_AGVs[i].JumpPoints[j].Y],
_rectangles[_AGVs[i].JumpPoints[j + 1].X][_AGVs[i].JumpPoints[j + 1].Y]
);
_AGVs[i].Paths[j] = line;
}
}
for (int i = 0; i < _startPos.Count; i++)
{
if ((c - 1) > 0)
{
Array.Resize(ref _AGVs[i].Paths, c - 1); //resize of the _AGVs steps Table
}
}
if (_loads != 0)
{
tree_stats.Nodes[2].Text = "Remaining loads: " + _loads;
}
else
{
tree_stats.Nodes[2].Text = "Remaining loads: ";
}
Invalidate();
}
//Path-planner for collecting all the remaining Loads in the Grid
private void GetNextLoad(int whichAgv)
{
aGVIndexToolStripMenuItem.Checked = false;
GridPos endPos = new GridPos();
//finds the End point and uses it's coordinates as the starting coords for every AGV
for (var widthTrav = 0; widthTrav < Globals.WidthBlocks; widthTrav++)
{
for (var heightTrav = 0; heightTrav < Globals.HeightBlocks; heightTrav++)
{
if (_rectangles[widthTrav][heightTrav].BoxType == BoxType.End)
{
try {
_startPos[whichAgv] = new GridPos(widthTrav, heightTrav);
_a = _startPos[whichAgv].X;
_b = _startPos[whichAgv].Y;
} catch { }
}
}
}
List <GridPos> loadPos = new List <GridPos>();
for (var i = 0; i < Globals.WidthBlocks; i++)
{
for (var j = 0; j < Globals.HeightBlocks; j++)
{
if (_rectangles[i][j].BoxType == BoxType.Load)
{
_searchGrid.SetWalkableAt(new GridPos(i, j), false);
}
//places the available AND the temporarily trapped loads in a list
if (_isLoad[i, j] == 1 || _isLoad[i, j] == 4)
{
loadPos.Add(new GridPos(i, j));
}
}
}
loadPos = CheckForTrappedLoads(loadPos, new GridPos(_a, _b), true); //scans the loadPos list to check which loads are available
if (loadPos.Count == 0)
{
_AGVs[whichAgv].HasLoadToPick = false;
return;
}
_isLoad[loadPos[0].X, loadPos[0].Y] = 3;
_AGVs[whichAgv].MarkedLoad = new Point(loadPos[0].X, loadPos[0].Y);
_loads--;
endPos = loadPos[0];
//Mark all loads as unwalkable,except the targetted ones
for (var m = 0; m < loadPos.Count; m++)
{
_searchGrid.SetWalkableAt(loadPos[m], false);
}
_searchGrid.SetWalkableAt(loadPos[0], true);
//creates the path between the AGV (which at the moment is at the exit) and the Load
_jumpParam.Reset(_startPos[whichAgv], endPos);
List <GridPos> jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight);
_AGVs[whichAgv].JumpPoints = jumpPointsList; //adds the result from A* to the AGV's
//embedded List
//Mark all loads as unwalkable
for (var m = 0; m < loadPos.Count; m++)
{
_searchGrid.SetWalkableAt(loadPos[m], false);
}
int c = 0;
for (short i = 0; i < _startPos.Count; i++)
{
c += _AGVs[i].JumpPoints.Count;
if ((c - 1) > 0)
{
Array.Resize(ref _AGVs[i].Paths, c - 1);
}
}
for (int j = 0; j < _AGVs[whichAgv].JumpPoints.Count - 1; j++)
{
GridLine line = new GridLine(
_rectangles
[_AGVs[whichAgv].JumpPoints[j].X]
[_AGVs[whichAgv].JumpPoints[j].Y],
_rectangles
[_AGVs[whichAgv].JumpPoints[j + 1].X]
[_AGVs[whichAgv].JumpPoints[j + 1].Y]
);
_AGVs[whichAgv].Paths[j] = line;
}
//2nd part of route: Go to exit
int oldC = c - 1;
_jumpParam.Reset(endPos, _startPos[whichAgv]);
jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight);
_AGVs[whichAgv].JumpPoints.AddRange(jumpPointsList);
c = 0;
for (int i = 0; i < _startPos.Count; i++)
{
c += _AGVs[i].JumpPoints.Count;
if ((c - 1) > 0)
{
Array.Resize(ref _AGVs[i].Paths, oldC + (c - 1));
}
}
for (short i = 0; i < _startPos.Count; i++)
{
for (int j = 0; j < _AGVs[i].JumpPoints.Count - 1; j++)
{
GridLine line = new GridLine(
_rectangles
[_AGVs[i].JumpPoints[j].X]
[_AGVs[i].JumpPoints[j].Y],
_rectangles
[_AGVs[i].JumpPoints[j + 1].X]
[_AGVs[i].JumpPoints[j + 1].Y]
);
_AGVs[i].Paths[j] = line;
}
}
Invalidate();
}
//Basic path planner function
private void Redraw()
{
return;
bool start_found = false;
bool end_found = false;
mapHasLoads = false;
GridPos endPos = new GridPos();
pos_index = 0;
startPos = new List <GridPos>(); //list that will be filled with the starting points of every AGV
AGVs = new List <Vehicle>(); //list that will be filled with objects of the class Vehicle
loadPos = new List <GridPos>(); //list that will be filled with the points of every Load
//Double FOR-loops to scan the whole Grid and perform the needed actions
for (int i = 0; i < Globals._WidthBlocks; i++)
{
for (int j = 0; j < Globals._HeightBlocks; j++)
{
if (m_rectangles[i][j].boxType == BoxType.Wall)
{
searchGrid.SetWalkableAt(new GridPos(i, j), false);//Walls are marked as non-walkable
}
else
{
searchGrid.SetWalkableAt(new GridPos(i, j), true);//every other block is marked as walkable (for now)
}
if (m_rectangles[i][j].boxType == BoxType.Load)
{
mapHasLoads = true;
searchGrid.SetWalkableAt(new GridPos(i, j), false); //marks every Load as non-walkable
isLoad[i, j] = 1; //considers every Load as available
loadPos.Add(new GridPos(i, j)); //inserts the coordinates of the Load inside a list
}
if (m_rectangles[i][j].boxType == BoxType.Normal)
{
m_rectangles[i][j].onHover(Globals.boxDefaultColor);
}
if (m_rectangles[i][j].boxType == BoxType.Start)
{
if (beforeStart)
{
searchGrid.SetWalkableAt(new GridPos(i, j), false); //initial starting points of AGV are non walkable until 1st run is completed
}
else
{
searchGrid.SetWalkableAt(new GridPos(i, j), true);
}
start_found = true;
AGVs.Add(new Vehicle(this));
AGVs[pos_index].ID = pos_index;
startPos.Add(new GridPos(i, j)); //adds the starting coordinates of an AGV to the StartPos list
//a & b are used by DrawPoints() as the starting x,y for calculation purposes
a = startPos[pos_index].x;
b = startPos[pos_index].y;
if (pos_index < startPos.Count)
{
startPos[pos_index] = new GridPos(startPos[pos_index].x, startPos[pos_index].y);
pos_index++;
}
}
if (m_rectangles[i][j].boxType == BoxType.End)
{
end_found = true;
endPos.x = i;
endPos.y = j;
}
}
}
if (!start_found || !end_found)
{
return; //will return if there are no starting or end points in the Grid
}
pos_index = 0;
if (AGVs != null)
{
for (int i = 0; i < AGVs.Count(); i++)
{
if (AGVs[i] != null)
{
AGVs[i].Status.Busy = false; //initialize the status of AGVs, as 'available'
}
}
}
startPos = NotTrappedVehicles(startPos, endPos); //replaces the List with all the inserted AGVs
//with a new one containing the right ones
if (mapHasLoads)
{
KeepValidLoads(endPos); //calls a function that checks which Loads are available
}
//to be picked up by AGVs and removed the trapped ones.
//For-loop to repeat the path-finding process for ALL the AGVs that participate in the simulation
for (int i = 0; i < startPos.Count; i++)
{
if (loadPos.Count != 0)
{
loadPos = CheckForTrappedLoads(loadPos, endPos);
}
if (loadPos.Count == 0)
{
mapHasLoads = false;
AGVs[i].HasLoadToPick = false;
}
else
{
mapHasLoads = true;
AGVs[i].HasLoadToPick = true;
}
if (AGVs[i].Status.Busy == false)
{
List <GridPos> JumpPointsList;
switch (mapHasLoads)
{
case true:
//====create the path FROM START TO LOAD, if load exists=====
for (int m = 0; m < loadPos.Count; m++)
{
searchGrid.SetWalkableAt(loadPos[m], false); //Do not allow walk over any other load except the targeted one
}
searchGrid.SetWalkableAt(loadPos[0], true);
//use of the A* alorithms to find the path between AGV and its marked Load
jumpParam.Reset(startPos[pos_index], loadPos[0]);
JumpPointsList = AStarFinder.FindPath(jumpParam, nud_weight.Value);
AGVs[i].JumpPoints = JumpPointsList;
AGVs[i].Status.Busy = true;
//====create the path FROM START TO LOAD, if load exists=====
//======FROM LOAD TO END======
for (int m = 0; m < loadPos.Count; m++)
{
searchGrid.SetWalkableAt(loadPos[m], false);
}
jumpParam.Reset(loadPos[0], endPos);
JumpPointsList = AStarFinder.FindPath(jumpParam, nud_weight.Value);
AGVs[i].JumpPoints.AddRange(JumpPointsList);
//marks the load that each AGV picks up on the 1st route, as 3, so each agv knows where to go after delivering the 1st load
isLoad[loadPos[0].x, loadPos[0].y] = 3;
AGVs[i].MarkedLoad = new Point(loadPos[0].x, loadPos[0].y);
loadPos.Remove(loadPos[0]);
//======FROM LOAD TO END======
break;
case false:
jumpParam.Reset(startPos[pos_index], endPos);
JumpPointsList = AStarFinder.FindPath(jumpParam, nud_weight.Value);
AGVs[i].JumpPoints = JumpPointsList;
break;
}
}
pos_index++;
}
int c = 0;
for (int i = 0; i < startPos.Count; i++)
{
c += AGVs[i].JumpPoints.Count;
}
for (int i = 0; i < startPos.Count; i++)
{
for (int j = 0; j < AGVs[i].JumpPoints.Count - 1; j++)
{
GridLine line = new GridLine
(
m_rectangles[AGVs[i].JumpPoints[j].x][AGVs[i].JumpPoints[j].y],
m_rectangles[AGVs[i].JumpPoints[j + 1].x][AGVs[i].JumpPoints[j + 1].y]
);
AGVs[i].Paths[j] = line;
}
}
for (int i = 0; i < startPos.Count; i++)
{
if ((c - 1) > 0)
{
Array.Resize(ref AGVs[i].Paths, c - 1); //resize of the AGVs steps Table
}
}
Invalidate();
}
