Path Finding Algorithms

Path finding is the problem of finding the shortest route between two given points.

Path-finding is closely related to graph theory, and solves the shortest route/path problem based on some criteria such as which one is shortest/cheapest path.

Table of contents / Different Path finding Algorithms:

  1. Breadth First Search and Depth First Search
  2. Dijkstra's Algorithm
  3. Bellman Ford Algorithm
  4. A* Search Algorithm

Let us get started with Path finding Algorithms.

1. Breadth First Search and Depth First Search


Our goal is to start from some vertex s in a connected graph G and systematically visit every othervertex in G. One reason to do this may be to look for a particular vertex in G and find a path from your start vertex s to the target vertex. There are two main approaches to searching through the graph: breadth-first and depth-first. The two algorithms follow the same outline.


  1. Initialize a seen set and a to-visit list both containing just the
    starting vertex s.
  2. While the to-visit list is not empty:
    (a) Remove the first vertex n from to-visit
    (b) For each neighbor vertex of n, if it is not in seen, add it to seen
    and to-visit(Optionally add parent pointers back to n if you want to
    recover path)




  1. Initalize,
    a. set visited[1..n]=false
    b. set src and dst
    c. set queue.push(1)
  2. Loop until queue is not empty
    a. set current = queue.front()
    b. queue.pop()
  3. For each node v in neighbour(current)
    a. if visited[v], then continue
    b. if dst == v, then destination node is
    found, return.
    c. queue.push(v), repeat 2



  1. Initalize,
    a. set visited[1..n] = false
    b. set src and dst node
  2. For each node v in Graph
    For each node u in neighbour(v)
    a. if visited[u], then continue
    b. if dst == u or dst == v, then destination is
    reached, return
    c. visited[u]= true

Complexity: O(|V| + |E|)


#include <bits/stdc++.h>
using namespace std;

vector<int> arr[100];
bool visited[100];

bool dfs(int src, int dst){

    if(src == dst){
        cout<<" Found \n";
        return true;

    for(auto next : arr[src]){
        if(visited[next]) continue;
        cout<<next<<" ";
        dfs(next, dst);
    visited[src] = true;

bool bfs(int src, int dst){

    queue<int> q;

        int cur = q.front();

        for(auto a: arr[cur]){
            if(visited[a]) continue;
            if(a == dst){
                cout<<" Found \n";
                return true;
        visited[cur] = true;

int main(){
    int n, e;// number of vertices and edges
    int x, y;// edge pair x,y


    for(int i=1; i<= n; i++){
        visited[i] = false;
    dfs(1, 6);

    for(int i=1; i<= n; i++){
        visited[i] = false;
    bfs(1, 6);

    return 0;

2. Dijkstra's Algorithm


It is an algorithm used to find shortest path between two given nodes.
Original implementation, finds ths shortest path from single source node to every other node in the graph. However, we can stop the algorithm once the destination node is reached.


We will try to find shortest path to every other node, that will include destination node also. Once specified starting node, dijkstra starts exploring connected nodes to source node and selects one with lowest edge cost. It then marks source node as visited, updates the distances of connected nodes (initially, all nodes were set to INT_MAX distance or infinity).

In order to find node with minimum cost edge we are using priority queue.Once edge is visited, pop it from priority queue. Update source vertex to minimum cost edge vertex and repeat procedure until all vertices are visited.


dijkstra(Node src, Node dst)

  1. Initalize,
    a. visited[total_nodes]= false
    b. distance[src..dst]= INT_MAX
    c. insert (src, 0) in priority_queue, where src = source node,
    0 = src node distance set to zero.
  2. repeat until all nodes are visited or destination node is not visited:
    a. for each neighbour v of src
    I) w = distance[src] + weight src to v
    II) if w < distance[v]. update distance[v],
    III) insert (v, distance[v]) in priority_queue
    IV) set visited[w] to true
    V) remove src distance from priority_queue
    b. Get next node with minimum distance
    c. Check whether all nodes are visited or if defined
    destination is visited then break; otherwise continue


a. Space Complexity - O(V)
b. Time Complexity - O(V^2) or O(|V| + |E log(V)|)


#include <bits/stdc++.h>
using namespace std;

// Dijkstra
template<typename T>
class Graph{


        typedef T vertex;
        typedef unordered_map<vertex, int> EdgeWeight;
        typedef unordered_map<vertex, EdgeWeight> tGraph;

        typedef pair<int, pair<vertex, vertex>> WVPair; 
        // vertex to weight pair

        typedef vector<WVPair> VWVPair;
        // vector of vertets-weight pair

        typedef priority_queue<WVPair, VWVPair, greater<WVPair>> 

        typedef typename tGraph::iterator gIterator;
        typedef typename EdgeWeight::iterator wIterator;

        tGraph G;
        unordered_map<vertex, bool> visited;
        Priority priority;

        Graph(): G(), visited(), priority(){};

        void addEdge(vertex start, vertex end, int weight = 1){
            gIterator sIter = G.find(start);
            if(sIter == G.end()){
                sIter = G.find(start);

            gIterator eIter = G.find(end);
            if(eIter == G.end()){
                eIter = G.find(end);
            G[start].insert(make_pair(end, weight));
            G[end].insert(make_pair(start, weight));

        void Dijkstra(vertex start, vertex end){
            unordered_map<vertex, bool> visited;
            for(gIterator gIter = G.begin(); gIter!=G.end(); gIter++)
                visited[gIter->first] = false;

            unordered_map<vertex, vertex> parent;
            unordered_map<vertex, int> distance;
            for(gIterator gIter=G.begin(); gIter!=G.end(); gIter++){
                distance[gIter->first] = INT_MAX;

            priority.push(make_pair(0, make_pair(start, start)));

            unordered_map<vertex, vertex> came_from;

            while(!priority.empty() and !visited[end]){

                // pop vertex with less weight, initlally start vertex is assumed to be came from itself
                pair<int, pair<vertex, vertex>> temp =; // pair<int weight, vertex u, vertex v> // u came from v


                // find adjacent vertex's to current temp vertex in graph 
                gIterator gIter = G.find(temp.second.first);
                // set<vrtex u, set<vertes v, int weight>>

                // mark temp vertex as visisted
                visited[temp.second.first] = true;

                came_from[temp.second.first] = temp.second.second;

                // iterate over adjacent vertices, so called edges
                for(wIterator wIter = gIter->second.begin(); wIter!=gIter->second.end(); wIter++){

                    // calculate distance from current temp vertex to current adjacent vertex
                    int temp_wt = wIter->second + temp.first;

                    // see if current adjacent veretx is already visited
                    if(visited[wIter->first]) continue;

                    // see if current adjacent vertex distance is already less than current distacne 
                    if(temp_wt < distance[wIter->first]){

                        // update to new distance
                        distance[wIter->first] = temp_wt;

                        // storing seleted vertex and it's adjacent vertex
                            make_pair(wIter->first, temp.second.first)));

            // print, visited vertices in order where u is comming from v

            cout<<"to "<<"from"<<endl;
            for(auto a: came_from){
                cout<<a.first<<" "<<a.second<<endl;


        void print(){
            for(gIterator gIter = G.begin(); gIter!=G.end(); gIter++){
                cout<<"V: "<<gIter->first<<" "<<endl;
                EdgeWeight edgeWeight = gIter->second;
                for(wIterator uIter = edgeWeight.begin(); uIter!=edgeWeight.end(); uIter++)
                    cout<<uIter->first<<" "<<uIter->second<<", ";

int main(){
    Graph<int> g;

    g.Dijkstra(1, 6);

    return 0;


a. Network Routing Protocols.
b. As a subroutine in other algorithms, for example Johnsons Algorithm.

3. Bellman Ford Algorithm


Bellman-Ford algorithm is an algorithm to find shortest path from single source vertex to all other vertex in the graph. The algoithm itself is slower than Dijkstras's algorithm, but unlike dijkstra it can be applied on negative weight cycle. It is also used to detect negative edge cycle in the graph. Negative edge cycle is where sum of weights of edges in a cycle is negative.

If negative edge cycle is present in the graph, then no path can be traced for the graph.


Algorithm starts with iterating (|V| - 1) times over all edges and relaxing edge cost. Relaxing an edge means computing cost for a node and updating them only if existing cost is larger.

Let's consider a graph with n number of nodes. We will relax all edges in the graph n-1 times.

We will start with source node, initalizing all nodes to unvisited true and distance set to infinity except start node distance set to 0.

Iterate on edges and relax them one at a time, update their distance and mark them visited.

After completing each iteration reset visited nodes.

When all iterations are done for (|V|-1) times, rotate once more to confirm for whether path is found or not. If any of the distances still changes in the relaxation process, that means we have negative edge cycle in graph and path cannot be found for such graph.


bellmanFord(Node src, Node dst)

  1. Loop for (|V| - 1) times
  2. Initalize,
    a. visited[0...total_nodes-1] = false
    b. distance[src] = 0, distance[1...total_nodes-1] = INT_MAX
    c. priority_queue.insert(pair(0, src))
  3. Relax edges just like we did for Dijkstra,
    a. for each neighbour v of src
    b. src_to_v = distance[v] + distance[src]
    c. if src_to_v < distance[v], update distance[v] = src_to_v
    d. mark src node, visited[src] = true and src = v. Repeat (3.a)
  4. Reset visited[0..total_nodes] = false. Repeat (2).
  5. Loop once more to check for negative edges.
    a. if distance array change for any vertex then: negative loop exist.
    else: path exists.


a. Space Complexity - O(V)
b. Time Complexity - O(|V|.|E|)

|V| is number of vertices and
|E| is number of edges


a. Routing protocol
b. Network flow analysis

4. A* Search Algorithm


A*, also pronounced as A star algorithm. It is seen as extension of Dijkstra algorithm. It uses the heuristics to help it find the goal location. In computer science, Heuristic's are used to solve the problems with some approximation.They help in speeding up the algorithm. A* was invented by robotics researchers for robot path planning.


As we discussed earlier, A* is kind of similar to Dijkstra algorithm. There are other names given to it such as best-first-search. Best-First-Search, it means that it will expand the graph by selecting the nodes which are more promising in terms of finding the goal/destination node. To select the most promising nodes, best-first-search uses heuristics.

  1. Heuritics function is defined by h(n),
    it represents distance of goal node from current node in the graph.
  2. There is another function g(n), which represents
    distance between start node and current node.

The algorithms tries to minimize the below function and finds the promising nodes to reach to the goal node.

f(n) = g(n) + h(n)

In these case, we will be using grid based map using 2d matrix. It will be easier to understand than normal list based graph. We define start location and destination location on matrix. We use priority queue to get lowest cost next node from current node. We will use four direction to explore graph that will include left, right, up and down only. For moving from one node (cell of the matrix) to other, we will be using some already defined weight (say w=5).

Initially, we will set priority of current node that is start node to 0 and push it on priority queue.

Algorithm will go into a loop unitl priority queue becomes empty.

Now using element from priority queue, we iterate over next nodes connected to current node (1,1) and pop it from priority queue.

The list of next nodes are explored by using four direction ((1,0),(0,1),(-1,0),(0,-1)). For current node (1,1), the list of next nodes are (2,1), (1,2), (0,1), (1,0).

For each of the node, we will check whether they are out of bounds or not.
In the next step, new cost for the next node is calculated using defined weight (w=5).

New cost will be only effective if by using heuritic some approximation is added. Using calculated value for f(n) of next node, a pair of priority value (i.e. f(n)+new cost ) and current node is inserted into priority queue.

The loop continues until either priorit queue is empty or destination location matches current location.


  1. astar(Node src, Node dst)
  2. Initialize,
    a. direction array, {(1,0), (0,1), (-1,0), (0,-1)}
    b. min priority_queue, insert(pair(0, src)) defines cost to node
    c. cost array, cost[src] = 0
  3. Loop until priority queue is not empty or dst node is not reached:
    a. Node current = priority_queue.pop().second, get minimum cost node
    b. neighbour[0...3] = current + direction[0...3]
    c. For each neighbour[0...3] v of current:
    d. new_cost = cost[v] + weight, where weight constant cost for moving
    to next node (example weight = 5)
    e. priority = heuritic (i.e. cost of destination node) + new_cost
    f. set cost[v] = new_cost,
    insert(pair(priority, v)) in priority queue


a. Space Complexity - A star does store all the generated nodes in
memory which is also the drawback of algorithm. O(|E|).
b. Time Complesity - O(|E| + |V|) or O(b^d) b is branching factor.


a. Path finding applications such as video games

With this article at OpenGenus, you must have a good idea of the basic Path Finding algorithms.