Ejemplo n.º 1
0
Archivo: AStar.cs Proyecto: Rolf321/p4
        /// <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);
        }
Ejemplo n.º 2
0
        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();
            }
        }