/// <summary>
/// The A*-pathfinding algorithm.
/// </summary>
/// <param name="inputVehicle"></param>
/// <param name="graph"></param>
public static void DoMagicStuff(Vehicle inputVehicle, RoadNetwork graph)
{
/* Method description (A* Pathfinding):
*
* Prefix
* - Reset all lists and dictionaries.
*
* 1. Populate dictionaries _gScore and _fScore.
* 2. Take start-node from parsed Vehicle and
* include it in _openSet, and set its _gScore to 0.
* 3. Calculate heuristics for start.
* 4. While-loop: (_openSet not empty).
* | a. Current (Connection) = lowest _fScore from _openSet.
* | b. If Current is Goal = HURRAY (Give path to input (Vehicle)
* | and return out of method).
* | c. Remove Current from _openSet, add Current to _closedSet.
* | d. For each neighbour in Current.
* | | i. If neighbour is in _closedSet, GOTO next iteration.
* | | ii. Add neighbour to _openSet if not already there.
* | | iii. Calculate weight from current to neighbour, store
* | | this value temporarily.
* | | iv. If calculated wight is not lower than _gScore[neighbour]
* | | then GOTO next iteration.
* | | v. _cameFrom[neighbour] = Current.
* | | vi. _gScore[neighbour] = calculated value from step iii.
* | | vii. _fScore[neighbour] = _gScore[neighbour] + heuristics(neighbour).
* 5. _openSet has been emptied, and no route from start to finish was found.
*/
// Prefix
_closedSet.Clear();
_openSet.Clear();
_cameFrom.Clear();
_gScore.Clear();
_fScore.Clear();
// 1
foreach (Connection c in graph.Connections)
{
_gScore.Add(c, double.PositiveInfinity);
_fScore.Add(c, double.PositiveInfinity);
}
// 2
_openSet.Add(inputVehicle.StartLocation);
_gScore[inputVehicle.StartLocation] = 0;
// 3 (Needs heuristics as well. Can be added when Heuristics() is done.)
_fScore[inputVehicle.StartLocation] = _gScore[inputVehicle.StartLocation];
// 4
while (_openSet.Any())
{
// a
Connection current = (from cPair in _fScore orderby cPair.Value select cPair.Key).Intersect(_openSet).First();
// b
if (current == inputVehicle?.Destination)
{
//
inputVehicle.Route = Reconstruct_path(current);
return;
}
// c
_openSet.Remove(current);
_closedSet.Add(current);
// d
var neighbours = graph.Connections.Where(c => c.Roads.Intersect(current.Roads).Any()).ToList();
foreach (Connection neighbour in neighbours)
{
// i
if (_closedSet.Contains(neighbour))
{
continue;
}
// ii
if (!_openSet.Contains(neighbour))
{
_openSet.Add(neighbour);
}
// iii & iv
double tempWeight = Weight(current, neighbour) + _gScore[current];
if (tempWeight >= _gScore[neighbour])
{
continue;
}
// v, vi & vii
_cameFrom[neighbour] = current;
_gScore[neighbour] = tempWeight;
_fScore[neighbour] = _gScore[neighbour]; // Add heuristics() when it's implemented.
}
}
// 5
Console.WriteLine("WARNING: Shortest path unavailable for car: " + inputVehicle);
}
static void Main(string[] args)
{
Random R = new Random();
// setup block.
RoadNetwork roadNetwork = new RoadNetwork();
roadNetwork.Connections.Add(new Connection("UL"));
roadNetwork.Connections.Add(new Connection("UR"));
roadNetwork.Connections.Add(new Connection("DL"));
roadNetwork.Connections.Add(new Connection("DR"));
roadNetwork.Connections.Add(new Connection("M"));
roadNetwork.Roads.Add(new Road("UL_UR", 100000, 80));
roadNetwork.Roads.Add(new Road("UR_DR", 10000, 80));
roadNetwork.Roads.Add(new Road("DL_DR", 20000, 80));
roadNetwork.Roads.Add(new Road("UL_DL", 1000, 80));
roadNetwork.Roads.Add(new Road("UL_M", 11000, 50));
roadNetwork.Roads.Add(new Road("UR_M", 20000, 50));
roadNetwork.Roads.Add(new Road("M_DR", 5000, 50));
roadNetwork.Roads.Add(new Road("M_DL", 5000, 50));
roadNetwork.Connections[0].Roads.Add(roadNetwork.Roads[0]);
roadNetwork.Connections[0].Roads.Add(roadNetwork.Roads[3]);
roadNetwork.Connections[0].Roads.Add(roadNetwork.Roads[4]);
roadNetwork.Connections[1].Roads.Add(roadNetwork.Roads[0]);
roadNetwork.Connections[1].Roads.Add(roadNetwork.Roads[1]);
roadNetwork.Connections[1].Roads.Add(roadNetwork.Roads[5]);
roadNetwork.Connections[2].Roads.Add(roadNetwork.Roads[2]);
roadNetwork.Connections[2].Roads.Add(roadNetwork.Roads[3]);
roadNetwork.Connections[2].Roads.Add(roadNetwork.Roads[7]);
roadNetwork.Connections[3].Roads.Add(roadNetwork.Roads[1]);
roadNetwork.Connections[3].Roads.Add(roadNetwork.Roads[2]);
roadNetwork.Connections[3].Roads.Add(roadNetwork.Roads[6]);
roadNetwork.Connections[4].Roads.Add(roadNetwork.Roads[4]);
roadNetwork.Connections[4].Roads.Add(roadNetwork.Roads[5]);
roadNetwork.Connections[4].Roads.Add(roadNetwork.Roads[6]);
roadNetwork.Connections[4].Roads.Add(roadNetwork.Roads[7]);
// End of setup block.
// Release the automobiles!
List <Vehicle> vehicles = new List <Vehicle>
{
new Vehicle(roadNetwork.Connections[R.Next(0, roadNetwork.Connections.Count)])
};
//vehicles.Add(new Vehicle(roadNetwork.Connections[R.Next(0, roadNetwork.Connections.Count)]));
//vehicles.Add(new Vehicle(roadNetwork.Connections[R.Next(0, roadNetwork.Connections.Count)]));
//vehicles.Add(new Vehicle(roadNetwork.Connections[R.Next(0, roadNetwork.Connections.Count)]));
//vehicles.Add(new Vehicle(roadNetwork.Connections[R.Next(0, roadNetwork.Connections.Count)]));
// AStar class informal test:
vehicles[0].StartLocation = roadNetwork.Connections[0];
vehicles[0].Destination = roadNetwork.Connections[1];
AStar.DoMagicStuff(vehicles.First(), roadNetwork);
// Simulation loop:
while (true)
{
foreach (Vehicle v in vehicles)
{
Console.WriteLine(v.UpdateLocation(R.Next(), roadNetwork.Connections));
}
Console.ReadKey();
}
}