Code States #3 - Linked Array Based Priority Queue Implementation

Priority Queue

They are the most important aspect, which must be known by any roboticist. Priority queue are used in path planning algorithms in general and shortes path algorithm like Dijkstra's algorithm. There are three types of Priority Queue implementation

1. Using Linked list 
2. Using Linked array representation
3. Priority matrix representation - Space taxing as a lot of elements might be empty

In the next set of Path planning algorithms, I will be making extensive use of Priority Queue. So that we all are on the same page, and you have the prerequisite, I am posting the code for Priority Queue implementation using Linked array representation. Please look into the theory of Priority Queue for implementation of (1) and (2)

DISCLAIMER - 

Though I have taken enough care that no bug is in introduced, I have written it in half an hour. Though I have checked the correctness on run-time, there might be some insidious bug. Please comment them on the blog, so that I can correct them and publish the revised version.

Cheers

Harshal

Edit - The codes below get creepy due to parsing issues that i am yet to figure out. Alternatively, you can fork me on github repository
harpribotics.blogspot
Do Fork me if you like it. :)

//// Array based Priority Queue implementation - Harshal Priyadarshi
#include
#include

using namespace std;
 
void assignRandomLinks(int* link,int* info, int* prn, int & avail, int & start, int size){
 avail = 0;                              //means that avail points to the very first array location
 start = size + 1;                       // means that start does not point anywhere
 
 for(int i = 0; i< size; i++){
  *(link + i) = i+1;
  *(info + i) = 0;
  *(prn + i) = 0;
 }
}
// Note To the readers
// link = size + 1 -> start is not initialized
// link = size     -> that the end point of either the occupied or avail list has reached
void priorityInsert(int *info, int* prn, int* link, int & avail, int & start, int num, int numPr, int size)
{
 //**** search for the suitable place where prn is just greater than numPr. ****\\
 //**** insert the number before that array ****\\
 
 //// check if the queue is full 
 if(avail == size)                       // means that their is no available place
 {
  printf("No place left. Kindly delete some information\n");
  
 }
 else{
 
  //// check if this is the first number to be inserted
  if(start == size + 1) 
  {
   start = avail;
   avail = *(link + avail);
   *(link + start) = size; 
   *(info + start) = num;
   *(prn + start) = numPr;
   
  }
  
  else{
  
   int counter = start;            // initialize the counter to start
   int prevlink = start;
   
   //// run till the pointer is just greater than numPr or till endpoint is reached
   while(numPr >= *(prn + counter) && counter != size)
   {
    prevlink = counter;         // maintains info of before link
    counter = *(link + counter);// maintains info of after link       
   }
   //// this handles all cases 
   int temp2 = *(link + prevlink);
   *(link + prevlink) = avail;     // the before link points to the first available position
   *(info + avail) = num;          // the info of 1st available position is set to num
   *(prn + avail) = numPr;         // the prn of 1st available position is set to numPr
   int temp = *(link + avail);
   *(link + avail) = temp2;        // the avail array points to the after link
   avail = temp;                   // the avail pointer is set to next available location
  }
 }
}

void priorityDelete(int *info, int* prn, int* link, int & avail, int & start,int size )
{
 if(start == size + 1)                   //means that no element was inserted before
 {
  printf("There was no entry in the system. Please feed in values\n");
 }
 else{
  if(*(link + start) == size)         //if the deletion will lead to an empty queue
  {
   int temp = avail;               // the present avail pointer is stored
   avail = start;                  // the start pointer is recovered
   *(info + avail) = 0;            // the values for priority and info is initialized
   *(prn + avail) = 0;   
   *(link + avail) = temp;         // the avail -> next = previous avail
   start = size + 1;               // the start pointer points to some value not representative in the link . We chose (size + 1)
  }
  
  else{
   int temp = avail;               // the present avail pointer is stored
   avail = start;                  // the start pointer is recovered
   *(info + avail) = 0;            // the values for priority and info is initialized
   *(prn + avail) = 0;
   start = *(link + start);   
   *(link + avail) = temp;         // the avail -> next = previous avail
   
  }
 }
}

int main(){
 int size = 10;
 int info[size], prn[size], link[size];
 int * INFO = info;                      // pointer to info
 int * PRN = prn;                        // pointer to priority prn
 int * LINK = link;                      // pointer to link
 int  start;                             // start or head location 
 int  avail;                             // first place available location
 int num = 0;                            // number to insert
 int numPr = 0;                          // priority of number to be inserted
 int choice;                             // choice of deletion or insertion

 //// initialize links randomly and assign the suitable start and avail list
 assignRandomLinks(LINK,INFO,PRN ,avail, start, size);
 
 //// make your choice now
 printf("Press 1 for Priority insert and press 2 for Priority delete ");
 scanf("%d",& choice);
 
 while(num != 99)                        // insert 99 to end the loop
 {                       
  
  if(choice == 1)                     // priority insert
  {                                   
   printf("Insert the number \n");
   cin>>num;
   printf("Insert the priority of number %d \n",num);
   cin>>numPr;
   priorityInsert(INFO, PRN, LINK, avail, start, num, numPr,size);
  }
  
  if(choice == 2)                     // priority delete
  {
   priorityDelete(INFO, PRN, LINK, avail, start, size);
  }
  
  //// print the three arrays
  for(int i = 0; i< size; i++)
  {
   printf("%d %d %d\n",info[i],prn[i],link[i]); 
  }
  
 //// make the choice within the loop
 printf("Press 1 for Priority insert and press 2 for Priority delete ");
 scanf("%d",& choice);
 }
}

Code States # 2 : Breadth First Search Algorithm for Graph

Hello Again!
For those who are not aware of this new series on Code States, in short, here you will get all the relevant codes. It has a two way benefits. I get to keep my codes handy instead of in some misspelled , never to be found folder in some random directory of my system. You get to have the coded implementations of the modules for free. Not everyone wants to do low level coding. They want modules. This will surely help.

See Code States # 1 for Depth First Search Algorithm.

Applications of Breadth First Search:

1. Find the shortest path between two points in a greedy manner in an unweighted graph .
2. To check the number of connected components in a graph.

Some Real world applications:

• GPS Navigation systems: Navigation systems such as the Google Maps, which can give directions to reach from one place to another use BFS. They take your location to be the source node and your destination as the destination node on the graph. (A city can be represented as a graph by taking landmarks as nodes and the roads as the edges that connect the nodes in the graph.) BFS is applied and the shortest route is generated which is used to give directions or real time navigation.
• Computer Networks: Peer to peer (P2P) applications such as the torrent clients need to locate a file that the client (one who wants to download the file) is requesting. This is achieved by applying BFS on the hosts (one who supplies the file) on a network. Your computer is the host and it keeps traversing through the network to find a host for the required file (maybe your favourite movie).
• Facebook: It treats each user profile as a node on the graph and two nodes are said to be connected if they are each other's friends. Infact, apply BFS on the facebook graph and you will find that any two people are connected with each other by atmost five nodes in between. To say, that you can reach any random person in the world by traversing 5 nodes in between. (I did not run BFS on facebook graph, it is a well known fact).What do you think is the new facebook "Graph Search"? (It is not directly BFS, but a lot of modifications over classic graph search algorithms.)
• Web Crawlers: It is quite an interesting application. They can be used to analyze what all sites you can reach by following links randomly on a particular website. 

So have the code and enjoy. See you soon.

Edit ---- The parsing for blogspot is just something i wont appreciate a lot. You can instead find the codes in my github repository. Fork me there, to get the code. The link is just below.
harpribotics.blogspot

# include
# define max_node 50
using namespace std;

struct adjlist{
 int vertex;
 adjlist* next;
};

adjlist *adj[max_node];

int totNodes;

int queue[max_node]; 
int f = -1; // front queue pointer
int r = -1; // rear queue pointer

// to insert in the queue
void q_insert (int item){
 r = r + 1;
 queue[r] = item;
 if(f == -1)
  f = 0;
 
}

// to delete in the queue
int q_delete (){
 int delitem = queue[f];
 if(f == r)
  f = r = -1;
 else
  f = f + 1;
 return(delitem);
}

// to check if the queue is empty
int is_q_empty(){
    if(f==-1)
      return(1);
    else return(0);
}
// creat the adjascency list of the graph

void createGraph(){
 adjlist *newl,*last;
    int neighbours,neighbour_value;
    cout<<"\n\n---Graph Creation---\n\n";
    cout<<"Enter total nodes in graph : ";
    cin>>totNodes;
    for(int i=1;i<=totNodes;i++){
        last=NULL;
        cout<<"\nEnter no. of nodes in the adjacency list of node "< That is Total Neighbours of "<>neighbours;
        for(int j=1;j<=neighbours;j++){
            cout<<"Enter neighbour #"<>neighbour_value;
            newl=new adjlist;
            newl -> vertex = neighbour_value;
            newl -> next = NULL;
            if(adj[i] == NULL){
             adj[i] = newl;
             last = newl;
            }
            else{
             last -> next = newl;
             last = newl;
            }
        }
    }
}

void BFS_traversal(){
    adjlist *tmp;
    int N,v,start_node,status[max_node];//status arr for maintaing status.
 const int ready=1,wait=2,processed=3; //status of node.

    cout<<"Enter starting node : ";
    cin>>start_node;

    //step 1 : Initialize all nodes to ready state.
 for(int i=1;i<=totNodes;i++)
        status[i]=ready;

    //step 2 : put the start node in queue and change status.
    q_insert(start_node); //Put starting node into queue.
    status[start_node]=wait; //change it status to wait state.//step 3 : Repeat until queue is empty.
 while(is_q_empty()!=1){

        //step 4 : Remove the front node N of queue.//process N and change the status of N to//be processed state.
        N = q_delete(); //remove front node of queue.
        status[N]=processed; //status of N to processed.
        cout<<" "<vertex;
            if(status[v]==ready){//check status of N's neighbour.
                q_insert(v); //insert N's neighbour who are in ready state.
                status[v]=wait; //and make their status to wait state.
            }
            tmp=tmp->next;
        }
    }
}

int main(){
    
    cout<<"*****Breadth First Search Traversal*****\n";
    createGraph();
    cout<<"\n===BFS traversal is as under===\n";
    BFS_traversal();
    int n;
    cin>>n;
}

Code States # 1 - Depth First Search (DFS) algorithm for Graph

People get bogged down to writing codes, when all they want is the implementation. Not everyone wants to be a true coder, but wants its benefits. Coding is fun and i suggest all to go this line at some point of their life if not now. This is a new series of blog for those who need reference for various coding algorithm and for those who need all together coding modules. Please feel free to use these codes and distribute it.

This is a code for Depth First Search algorithm for Graph. It is quite an intuitive algorithm and has quite a crisp code. 

Depth First Search algorithm is used in many applications, some of them are as follows:

1. To find if the graph is bipartite or not. A graph is bipartite if it can be divided into two mutually exclusive sets whose union is the set itself, and any edge of the graph joins vertex of one set to that of other.
2. For an unweighted graph, DFS traversal of the graph produces the minimum spanning tree.
3. Detecting cycle in a graph 
   A graph has cycle if and only if we see a back edge during DFS. So we can run DFS for the graph and check for back edges. 
4. Maze Solving - Heard of that famous Mouse and cheese problem on the Maze. We can use DFS for that purpose. It is how we would have done it, if we were a mouse.
5. Path finding - To find a path between two points.

There are many more, but i will give you the chance to Google them yourself. For example, search google for what is a Minimum Spanning Tree (MST).

So here is the code. Enjoy. 

Edit ---- The parsing for blogspot is just something i wont appreciate a lot. You can instead find the codes in my github repository. Fork me there, to get the code. The link is just below.
harpribotics.blogspot
 

# include
# define max_node 50
using namespace std;

struct adjlist{
 int vertex;
 adjlist* next;
};

adjlist *adj[max_node];

int visited[max_node];  // exclusive for recursive DFS

int totNodes;

// creat the adjascency list of the graph

void createGraph(){
 adjlist *newl,*last;
    int neighbours,neighbour_value;
    cout<<"\n\n---Graph Creation---\n\n";
    cout<<"Enter total nodes in graph : ";
    cin>>totNodes;
    for(int i=1;i<=totNodes;i++){
        last=NULL;
        visited[i] = 0; // exclusively for recursive DFS 
        cout<<"\nEnter no. of nodes in the adjacency list of node "< That is Total Neighbours of "<>neighbours;
        for(int j=1;j<=neighbours;j++){
            cout<<"Enter neighbour #"<>neighbour_value;
            newl=new adjlist;
            newl -> vertex = neighbour_value;
            newl -> next = NULL;
            if(adj[i] == NULL){
             adj[i] = newl;
             last = newl;
            }
            else{
             last -> next = newl;
             last = newl;
            }
        }
    }
}
// this quite an intellectual and crisp code instead of the lengthy recursion
void TrueDFS_Traversal(int w){
 visited[w] = 1; cout<<" "<vertex]) TrueDFS_Traversal(adj[w] -> vertex);
 adj[w] = adj[w] -> next;
 } 
}
int main(){
    
    cout<<"*****Depth First Search Traversal*****\n";
    createGraph();
    cout<<"\n===True DFS traversal using recursion is as under==\n";
    // ask for the start node
    int start_node;
    cout<<"Enter starting node : ";
    cin>>start_node;
    TrueDFS_Traversal(start_node);
    int n;
    cin>>n;
}

Sensor Fusion #1 : Alpha Beta Gamma Filter

Sensors are prone to error, no matter how much cash you spend. One way is to spend a lot getting an extremely accurate sensor, while other, more favorable way is to filter out the noise and drift in the faulty reading to get an output as close as possible to the ideal case.

In line with this, let me introduce, the Hello World of Sensor fusion, Alpha Beta Gamma Filter (or Alpha Beta Filter, if not using the acceleration term)

The Alpha Beta Filter is quite an intuitive, yet a very simple concept to begin with. The best explanation that i myself found was on Wikipedia. So instead of delving further explaining the same thing again and again, let me just give you the appropriate link of Alpha Beta Gamma filter from Wikipedia. Please do read it faithfully. It is very insightful and clearly explained.

Algorithm Summary

Initialize.

• Set the initial values of state estimates x and v, using prior information or additional measurements; otherwise, set the initial state values to zero.
• Select values of the alpha and beta correction gains.

Update. Repeat for each time step ΔT:

  Project state estimates x and v.
  Obtain a current measurement of the output value
  Compute the residual r.
  Correct the state estimates.
  Send updated x and optionally v as the filter outputs

Now the major aspect is the implementation. I have implemented the entire filter on C++ with simulated noise and drift. The code is given below. Feel free to copy it and make sure you run it on your platform.

Edit ---- The parsing for blogspot is just something i wont appreciate a lot. You can instead find the codes in my github repository. Fork me there, to get the code. The link is just below.
harpribotics.blogspot
 

/* An alpha beta filter implementation by Harshal Priyadarshi
Blog - http://harpribotics.blogspot.com
*/

#include
#include