Agv_UI/dijkstra/dijkstra.h

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

#ifndef LIO_LIVOX_CPP_DIJKSTRA_DIJKSTRA_H_
#define LIO_LIVOX_CPP_DIJKSTRA_DIJKSTRA_H_


//Purpose: Implementation of Dijkstra's algorithm which finds the shortest
//path from a start node to every other node in a weighted graph.
//Time complexity: O(n^2)
#include <iostream>
#include <limits>
#include <vector>
#include <map>

#define MAXV 1000
#define EXP 0.0001
class EdgeNode{
 public:
    int key;
    float weight;
    EdgeNode *next;
    EdgeNode(int, float);
};


class Graph{
    bool directed;
 public:
    EdgeNode *edges[MAXV + 1];
    Graph(bool);
    ~Graph();
    void insert_edge(int, int, float, bool);
    void print();
};

void init_vars(bool discovered[], int distance[], int parent[]);

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

void get_shortest_path(int v, int parent[],std::vector<int>& path);

void print_distances(int start, float distance[]);

class PathMap
{
 public:
    typedef struct
    {
      float x;
      float y;
    }MapNode;
    typedef struct
    {
      int id1;
      int id2;
      bool direct;
    }MapEdge;
 public:
    PathMap();
    ~PathMap();

    void AddVertex(int id,float x,float y);
    bool AddEdge(int id1,int id2,bool directed);

    bool GetShortestPath(int id1,int id2,std::vector<int>& path,float& shortest_distance);

    bool GetVertexPos(int id,float& x,float& y);
    std::map<int,MapNode> GetNodes()const{return node_;};
    std::vector<MapEdge> GetEdges()const{return edge_;}

 protected:
    std::map<int,MapNode> node_;
    std::vector<MapEdge>  edge_;
    Graph *grapher_;
    int parent_[MAXV + 1];
    float distance_[MAXV + 1];


};



#endif //LIO_LIVOX_CPP_DIJKSTRA_DIJKSTRA_H_