Dijkstra 返回路径

Question

我一直在寻找一种方法来实现 A* 和 Dijkstra，以便能够获得最短路径并开始完成。

我从 SQL 数据库中检索节点和边的列表，将这些项目放入两个字典（节点和边）中，并以 node/edge id 作为键。

我用来测试的开始（148309）和结束（1483093）节点，return是一个结果，但是它访问了其他21个节点，应该return3个节点（108.75m）

尝试在下面的链接中使用伪代码，我设法让它找到了路径，但我正在努力通过回溯来获得它所采用的实际最短路径。下面的链接在他们的例子中没有提到这一点。

https://www.csharpstar.com/dijkstra-algorithm-csharp/

https://www.programmingalgorithms.com/algorithm/dijkstra's-algorithm/

https://www.dotnetlovers.com/article/234/dijkstras-shortest-path-algorithm

对象

public class Node
{
    public Node()
    {
        Edges = new Dictionary<long, Edge>();
    }

    public long Id { get; set; }

    public double Latitude { get; set; }

    public double Longitude { get; set; }

    /// <summary>
    /// The edges coming out of this node.
    /// </summary>
    public Dictionary<long, Edge> Edges { get; set; }

    public double DistanceFromStart { get; set; }

    public double DistanceToEnd { get; set; }

    public bool Visited { get; set; }

    /// <summary>
    /// Specified the distance in KM between this node and the
    /// specified node using their lat/longs.
    /// </summary>
    /// <param name="node"></param>
    /// <returns></returns>
    public double DistanceTo(ref Node node)
    {
        return DistanceHelper.DistanceTo(this, node);
    }
}

public class Edge
{
    public long UID { get; set; }

    public long StartNodeId { get; set; }

    public long EndNodeId { get; set; }

    public double Distance { get; set; }

    public Node EndNode { get; set; }

}

public class SPResult
{
    public double Distance { get; set; }

    public long[] Nodes { get; set; }

    public long[] Edges { get; set; }
}

到目前为止的代码。

public static Graph graph = new Graph();

    static void Main(string[] args)
    {
        Console.WriteLine("Starting");

        //Loads the nodes and edges from a SQL database.
        LoadInfrastructure(3);
        var res = graph.GetShortestPathDijkstra(1483099, 1483093);
        //var res = graph.GetShortestPathDijkstra(1483129, 3156256);

        Console.WriteLine("Done. Press any key to exit.");
        Console.ReadKey();
    }

   public class Graph
{

    public Graph()
    {
        Nodes = new Dictionary<long, Node>();
        Edges = new Dictionary<long, Edge>();
    }

    Dictionary<long, Node> Nodes { get; set; }
    Dictionary<long, Edge> Edges { get; set; }

    Dictionary<long, double> queue;

    Stopwatch stopwatch = new Stopwatch();
    public void AddNode(Node n)
    {
        if (Nodes.ContainsKey(n.Id))
            throw new Exception("Id already in graph.");

        Nodes.Add(n.Id, n);
    }

    public void AddEdge(Edge e)
    {
        if (Edges.ContainsKey(e.UID))
            throw new Exception("Id already in graph.");

        e.EndNode = Nodes[e.EndNodeId];
        Edges.Add(e.UID, e);
        Nodes[e.StartNodeId].Edges.Add(e.UID, e);
    }

    public SPResult GetShortestPathDijkstra(long start, long end)
    {
        return GetShortestPathDijkstra(Nodes[start], Nodes[end]);
    }

    public SPResult GetShortestPathDijkstra(Node start, Node end)
    {
        if (!Nodes.ContainsKey(start.Id))
            throw new Exception("Start node missing!");

        if (!Nodes.ContainsKey(end.Id))
            throw new Exception("End node missing!");

        Console.WriteLine($"Finding shortest path between {start.Id} and {end.Id}...");

        ResetNodes(null);
        stopwatch.Restart();

        Node current = start;
        current.DistanceFromStart = 0;
        queue.Add(start.Id, 0);

        while (queue.Count > 0)
        {
            long minId = queue.OrderBy(x => x.Value).First().Key;
            current = Nodes[minId];
            queue.Remove(minId);

            if (minId == end.Id)
            {
                current.Visited = true;
                break;
            }
            foreach (var edge in current.Edges.OrderBy(ee => ee.Value.Distance))
            {
                var endNode = edge.Value.EndNode;
                if (endNode.Visited)
                    continue;

                double distance = current.DistanceFromStart + edge.Value.Distance;

                if (queue.ContainsKey(endNode.Id))
                {
                    if (queue[endNode.Id] > distance)
                    {
                        queue[endNode.Id] = endNode.Id;
                        Nodes[endNode.Id].DistanceFromStart = distance;
                    }
                }
                else
                {
                    Nodes[endNode.Id].DistanceFromStart = distance;
                    queue.Add(endNode.Id, distance);
                }
            }

            current.Visited = true;
        }

        stopwatch.Stop();
        Console.WriteLine($"Found shortest path between {start.Id} and {end.Id} in {stopwatch.ElapsedMilliseconds}ms.");
        Debug.WriteLine($"Found shortest path between {start.Id} and {end.Id} in {stopwatch.ElapsedMilliseconds}ms.");

**//Get path used.**

        var rr = Nodes.Values.Where(nn => nn.Visited).OrderBy(nn => nn.DistanceFromStart).ToList();

        return null;
    }

    public SPResult GetShortestPathAstar(long start, long end)
    {
        return GetShortestPathAstar(Nodes[start], Nodes[end]);
    }

    public SPResult GetShortestPathAstar(Node start, Node end)
    {
        ResetNodes(end);
        start.DistanceFromStart = 0;

        throw new NotImplementedException();
    }

    private void ResetNodes(Node endNode)
    {
        queue = new Dictionary<long, double>();
        foreach (var node in Nodes)
        {
            node.Value.DistanceFromStart = double.PositiveInfinity;
            node.Value.Visited = false;

            if (endNode != null)
                node.Value.DistanceToEnd = node.Value.DistanceTo(ref endNode);
        }
    }

}

Answer 1

我设法使用 YouTube 视频通过伪代码并实现了 Dijkstra 和 A* 算法。

https://www.youtube.com/watch?v=nhiFx28e7JY

https://www.youtube.com/watch?v=mZfyt03LDH4

Program.cs （片段）

LoadInfrastructureFromSQL();
var resA5 = graph.GetShortestPathAstar(startId, endId);

节点

public class Node
{
    public Node()
    {

    }

    public Node(long id, double lat, double lon) : this()
    {
        Id = id;
        Latitude = lat;
        Longitude = lon;
    }

    public long Id { get; set; }

    public double Latitude { get; set; }

    public double Longitude { get; set; }

    public double Gcost { get; set; }

    public double Hcost { get; set; }

    public double Fcost => Gcost + Hcost;

    public Node Parent { get; set; }

    /// <summary>
    /// The edges coming out of this node.
    /// </summary>
    public Dictionary<long, Edge> Edges { get; set; }


    public void AddEdges(Edge e)
    {
        if (Edges == null)
            Edges = new Dictionary<long, Edge>();

        if (Edges.ContainsKey(e.UID))
            throw new Exception($"Edge id {e.UID} already exists.");

        Edges.Add(e.UID, e);

    }
    public double DistanceTo(Node point)
    {
        double p = 0.017453292519943295;
        double a = 0.5 - Math.Cos((point.Latitude - Latitude) * p) / 2 + Math.Cos(Latitude * p) * Math.Cos(point.Latitude * p) * (1 - Math.Cos((point.Longitude - Longitude) * p)) / 2;
        return 12742 * Math.Asin(Math.Sqrt(a));
    }

}

边缘

public class Edge
{
    public Edge()
    {

    }

    public Edge(long uid, long id, double distance)
    {
        UID = uid;
        WayId = id;
        Distance = distance;
    }

    public Edge(long uid, long id, double distance, long startNode, long endNode) : this(uid, id, distance)
    {
        StartNodeId = startNode;
        EndNodeId = endNode;
    }

     /// <summary>
    /// Unique way id for every single edge.
    /// </summary>
    public long UID { get; set; }

    /// <summary>
    /// Duplicate edges will share the same WayId i.e. if the way is bi-directional.
    /// </summary>
    public long WayId { get; set; }

    public long StartNodeId { get; set; }

    public long EndNodeId { get; set; }        

    public Node EndNode { get; set; }

    public double Distance { get; set; }
}

结果（可选）

public class SPResult
{
    public double Distance { get; set; }

    public Node[] Nodes { get; set; }

    public long NodesChecked { get; set; }

    public Edge[] Edges { get; set; }

    public long CalculationTime { get; set; }
}

图形与算法

   //Routing algorithm taken from https://www.youtube.com/watch?time_continue=5&v=-L-WgKMFuhE

public class Graph
{
    public Graph()
    {
        Nodes = new Dictionary<long, Node>();
        Edges = new Dictionary<long, Edge>();
    }

    Dictionary<long, Node> Nodes { get; set; }
    Dictionary<long, Edge> Edges { get; set; }

    Dictionary<long, Node> open;
    Dictionary<long, Node> closed;

    Stopwatch stopwatch = new Stopwatch();
    public void AddNode(Node n)
    {
        if (Nodes.ContainsKey(n.Id))
            throw new Exception("Id already in graph.");

        Nodes.Add(n.Id, n);
    }


    public void AddEdge(Edge e)
    {           
        e.EndNode = Nodes[e.EndNodeId];
        Edges.Add(e.UID, e);
        Nodes[e.StartNodeId].AddEdges(e);
    }

    public Node FindClosestNode(double lat, double lon, int radius)
    {
        Node n = new Node();
        n.Latitude = lat;
        n.Longitude = lon;

        return FindClosestNode(n, radius);
    }

    public Node FindClosestNode(Node node, int radius)
    {
        Console.WriteLine($"Finding closest node [Latitude: {Math.Round(node.Latitude, 6)}, Longitude:{Math.Round(node.Longitude, 6)}]...");
        Stopwatch sw = new Stopwatch();
        sw.Start();
        var res = Nodes.Select(x => new { Id = x.Key, Distance = x.Value.DistanceTo(node) }).Where(nn => nn.Distance < radius).OrderBy(x => x.Distance).FirstOrDefault();

        if (res != null)
        {
            Debug.WriteLine($"Found nearest node in {sw.ElapsedMilliseconds}ms [{res.Id}]");
            return Nodes[res.Id];
        }

        Debug.WriteLine($"No nearest node {sw.ElapsedMilliseconds}ms [{res.Id}]");
        return null;
    }



    #region ROUTING
    public async Task<SPResult> GetShortestPathDijkstra(long start, long end)
    {
        return await GetShortestPathDijkstra(Nodes[start], Nodes[end]);
    }

    public async Task<SPResult> GetShortestPathDijkstra(Node start, Node end)
    {
        if (!Nodes.ContainsKey(start.Id))
            throw new Exception("Start node missing!");

        if (!Nodes.ContainsKey(end.Id))
            throw new Exception("End node missing!");

        Console.WriteLine($"Finding Dijkstra shortest path between {start.Id} and {end.Id}...");

        ResetNodes(null);
        stopwatch.Restart();

        open.Add(start.Id, start);
        start.Gcost = 0;
        Node current = start;

        while (true)
        {
            long lowest = open.OrderBy(qq => qq.Value.Fcost).FirstOrDefault().Key;
            current = Nodes[lowest];

            open.Remove(lowest);
            closed.Add(lowest, current);

            if (current.Id == end.Id)
                break;

            foreach (var neigh in current.Edges)
            {
                Node edgeEnd = neigh.Value.EndNode;
                if (closed.ContainsKey(edgeEnd.Id))
                    continue;

                double distance = current.Gcost + neigh.Value.Distance;
                if (distance < edgeEnd.Gcost || !open.ContainsKey(edgeEnd.Id))
                {
                    edgeEnd.Gcost = distance;
                    edgeEnd.Parent = current;

                    if (!open.ContainsKey(edgeEnd.Id))
                        open.Add(edgeEnd.Id, edgeEnd);                           
                }
            }
        }

        stopwatch.Stop();
        Console.WriteLine($"Found shortest path between {start.Id} and {end.Id} in {stopwatch.ElapsedMilliseconds}ms.");
        Debug.WriteLine($"Found shortest path between {start.Id} and {end.Id} in {stopwatch.ElapsedMilliseconds}ms.");

        SPResult spr = new SPResult();
        spr.CalculationTime = stopwatch.ElapsedMilliseconds;
        spr.Distance = current.Gcost;

        var traceback = Traceback(current);
        spr.Nodes = traceback.Item1;
        spr.Edges = traceback.Item2;
        spr.NodesChecked = closed.Count + open.Count;
        return spr;
    }

    public async Task<SPResult> GetShortestPathAstar(long start, long end)
    {
        return await GetShortestPathAstar(Nodes[start], Nodes[end]);
    }

    public async Task<SPResult> GetShortestPathAstar(Node start, Node end)
    {
        if (!Nodes.ContainsKey(start.Id))
            throw new Exception("Start node missing!");

        if (!Nodes.ContainsKey(end.Id))
            throw new Exception("End node missing!");

        Console.WriteLine($"Finding A* shortest path between {start.Id} and {end.Id}...");

        ResetNodes(end);
        stopwatch.Restart();

        start.Gcost = 0;
        Node current = start;
        open.Add(start.Id, current);            

        while (true)
        {
            if (open.Count() == 0)
                return null;

            long lowest = open.OrderBy(qq => qq.Value.Fcost).FirstOrDefault().Key;
            current = Nodes[lowest];

            open.Remove(lowest);
            closed.Add(lowest, current);

            if (current.Id == end.Id)
                break;

            foreach (var neigh in current.Edges)
            {
                Node edgeEnd = neigh.Value.EndNode;
                if (closed.ContainsKey(edgeEnd.Id))
                    continue;

                double distance = current.Gcost + neigh.Value.Distance;
                if (distance < (edgeEnd.Gcost) || !open.ContainsKey(edgeEnd.Id))
                {
                    edgeEnd.Gcost = distance;
                    edgeEnd.Hcost = current.DistanceTo(end);
                    edgeEnd.Parent = current;

                    if (!open.ContainsKey(edgeEnd.Id))
                        open.Add(edgeEnd.Id, edgeEnd);
                }
            }
        }

        stopwatch.Stop();
        Console.WriteLine($"Found shortest path between {start.Id} and {end.Id} in {stopwatch.ElapsedMilliseconds}ms.");
        Debug.WriteLine($"Found shortest path between {start.Id} and {end.Id} in {stopwatch.ElapsedMilliseconds}ms.");

        SPResult spr = new SPResult();
        spr.CalculationTime = stopwatch.ElapsedMilliseconds;
        spr.Distance = current.Gcost;

        var traceback = Traceback(current);
        spr.Nodes = traceback.Item1;
        spr.Edges = traceback.Item2;
        spr.NodesChecked = closed.Count + open.Count;
        return spr;
    }

    private (Node[],Edge[]) Traceback(Node current)
    {

        Stopwatch sw = new Stopwatch();
        sw.Start();
        List<Node> nodes = new List<Node>();
        List<Edge> edges = new List<Edge>();

        nodes.Add(current);
        while (current.Parent != null)
        {
            edges.Add(current.Parent.Edges.FirstOrDefault(ee => ee.Value.EndNodeId == current.Id).Value);
            current = current.Parent;
            nodes.Add(current);
        }
        Debug.WriteLine($"Traceback in {sw.ElapsedMilliseconds}ms.");
        return nodes.Reverse<Node>().ToArray();
    }

    private void ResetNodes(Node endNode)
    {
        open = new Dictionary<long, Node>();
        closed = new Dictionary<long, Node>();
        foreach (var node in Nodes)
        {
            node.Value.Gcost = double.PositiveInfinity;
            node.Value.Hcost = double.PositiveInfinity;
        }
    }

    #endregion
}

Dijkstra 返回路径

Dijkstra Returning Path

c#

dijkstra