Dijkstra Returning Path

Question

I've been looking for a way to implement both A* and Dijkstra to be able to get the shortest path and start to finish.

I retrieve a list of nodes and edges from a SQL database put the items into two dictionaries (Nodes and Edges) with the node/edge id as the keys.

The start (148309) and end (1483093) node I used to test, returns a result, but it visits 21 other nodes and should return 3 nodes (108.75m)

Attempting to use the pseduo code in the links below, I've managed to make it find the path but I'm struggling with the backtracing to get the actual shortest path it took. The links below don't mention this within their examples.

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

Objects

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; }
}

Code so far.

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

I managed to use a YouTube video to go through the pseudocode and implement both Dijkstra and A* algorithms.

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

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

Program.cs (Snippet)

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

Node

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));
    }

}

Edge

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; }
}

Result (Optional)

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; }
}

Graph and Algorithm

   //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
}