Agv_UI/dijkstra/dijkstra.cpp

//
// Created by zx on 22-11-29.
//

#include "dijkstra.h"
#include <math.h>

EdgeNode::EdgeNode(int key, float weight){
  this->key = key;
  this->weight = weight;
  this->next = NULL;
}

Graph::Graph(bool directed){
  this->directed = directed;
  for(int i = 1; i < (MAXV + 1); i ++){
    this->edges[i] = NULL;
  }
}

Graph::~Graph(){
  //to do
}

void Graph::insert_edge(int x, int y, float weight, bool directed){
  if(x > 0 && x < (MAXV + 1) && y > 0 && y < (MAXV + 1)){
    EdgeNode *edge = new EdgeNode(y, weight);
    edge->next = this->edges[x];
    this->edges[x] = edge;
    if(!directed){
      insert_edge(y, x, weight, true);
    }
  }
}

void Graph::print(){
  for(int v = 1; v < (MAXV + 1); v ++){
    if(this->edges[v] != NULL){
      std::cout << "Vertex " << v << " has neighbors: " << std::endl;
      EdgeNode *curr = this->edges[v];
      while(curr != NULL){
        std::cout << curr->key << std::endl;
        curr = curr->next;
      }
    }
  }
}

void init_vars(bool discovered[], float distance[], int parent[]){
  for(int i = 1; i < (MAXV + 1); i ++){
    discovered[i] = false;
    distance[i] = std::numeric_limits<float>::max();
    parent[i] = -1;
  }
}

void dijkstra_shortest_path(Graph *g, int parent[], float distance[], int start){

  bool discovered[MAXV + 1];
  EdgeNode *curr;
  int v_curr;
  int v_neighbor;
  float weight;
  float smallest_dist;

  init_vars(discovered, distance, parent);

  distance[start] = 0;
  v_curr = start;

  while(discovered[v_curr] == false){

    discovered[v_curr] = true;
    curr = g->edges[v_curr];

    while(curr != NULL){

      v_neighbor = curr->key;
      weight = curr->weight;

      if((distance[v_curr] + weight) < distance[v_neighbor]){
        distance[v_neighbor] = distance[v_curr] + weight;
        parent[v_neighbor] = v_curr;
      }
      curr = curr->next;
    }

    //set the next current vertex to the vertex with the smallest distance
    smallest_dist = std::numeric_limits<float>::max();
    for(int i = 1; i < (MAXV + 1); i ++){
      if(!discovered[i] && (distance[i] < smallest_dist)){
        v_curr = i;
        smallest_dist = distance[i];
      }
    }
  }
}

void get_shortest_path(int v, int parent[],std::vector<int>& path){
  if(v > 0 && v < (MAXV + 1) && parent[v] != -1){
    path.push_back( parent[v]) ;
    get_shortest_path(parent[v], parent,path);
  }

}

void print_distances(int start, float distance[]){
  for(int i = 1; i < (MAXV + 1); i ++){
    if(distance[i] != std::numeric_limits<float>::max()){
      std::cout << "Shortest distance from " << start << " to " << i << " is: " << distance[i] << std::endl;
    }
  }
}


PathMap::PathMap(){
  grapher_ = new Graph(false);
}
PathMap::~PathMap()
{
  delete grapher_;
}

void PathMap::AddVertex(int id,float x,float y)
{
  node_[id]={x,y};
  printf(" Add node %f,%f  nodes :%d\n",x,y,node_.size());
}
bool PathMap::AddEdge(int id1,int id2,bool directed)
{
  if(node_.find(id1)!=node_.end() && node_.find(id2)!=node_.end())
  {
    float x1=node_[id1].x;
    float y1=node_[id1].y;
    float x2=node_[id2].x;
    float y2=node_[id2].y;
    float distance=sqrt(pow(x2-x1,2)+pow(y2-y1,2))+EXP;
    grapher_->insert_edge(id1, id2, distance, directed);
    edge_.push_back({id1,id2,directed});
    printf(" Add edge %d-%d \n",id1,id2);
    return true;
  }
  return false;
}

bool PathMap::GetShortestPath(int id1,int id2,std::vector<int>& path,float& shortest_distance)
{
  if(id1==id2)
    return false;
  if(node_.find(id1)!=node_.end() && node_.find(id2)!=node_.end())
  {
    dijkstra_shortest_path(grapher_, parent_, distance_, id1);
    get_shortest_path(id2, parent_, path);
    shortest_distance=distance_[id2];
    return true;
  }
  return false;
}

bool PathMap::GetVertexPos(int id,float& x,float& y)
{
  if(node_.find(id)!=node_.end())
  {
    x=node_[id].x;
    y=node_[id].y;
    return true;
  }
  return false;
}