graph.hpp

#ifndef GRAPH_HPP
#define GRAPH_HPP

#include "API/Roadmap/node.hpp"
#include "API/Roadmap/edge.hpp"
#include "API/Trajectory/trajectory.hpp"

#ifndef COMPCO_HPP
class ConnectedComponent;
#endif

#include <map>

#ifndef _ROADMAP_H
struct node;
struct edge;
struct graph;
struct list_node;
struct list_edge;
#endif

class Graph
{
public:
  
        //contructors and destructor
        Graph() {}
        Graph(Robot* R, graph* ptGraph);
        Graph(Robot* R);
        Graph(graph* G);
        Graph(const Graph& G);
        ~Graph();
  
        bool isGraphChanged() { return m_graphChanged; }
        
        //graph* exportCppToGraphStruct(bool deleteGraphStruct = false);
        
        //Accessors
        graph* getGraphStruct() const;
        
        void setGraph(graph* G);
        
        Robot* getRobot() const;
        
        void setRobot(Robot* R);
        
        std::map<node*, Node*> getNodesTable();
        
        unsigned int getNumberOfNodes();
  
        std::vector<Node*> getNodes() const;
        std::vector<Node*> getSortedNodes() const;
        std::vector<Node*> getNotTrajNodes() const;
        
        Node* getNode(unsigned int i) const { return m_Nodes[i]; } 
        
        unsigned int getNumberOfEdges() const;
        
        std::vector<Edge*> getEdges() const;

    std::vector<Edge*> getEdges(std::tr1::shared_ptr<Configuration> q,bool from=false);
    std::vector<Edge*> getEdges(Node* nq,bool from=false);
        
        Edge* getEdge(unsigned int i)  const { return m_Edges[i]; } 
        
        /*
         * Get the number of compco in the grap
         */
        unsigned int getNumberOfCompco() const;
  
        ConnectedComponent* getConnectedComponent(int ith) const;
        
        std::vector<ConnectedComponent*> getConnectedComponents() const;
        
        Node* getNode(node* N);
        
        Node* getLastNode();
        
        std::string getName();
        
        void setName();
  
        void setName(std::string Name);
        
        bool equal(Graph* G);
  
        bool equalName(Graph* G);
        
        bool isInGraph(Node* N);
        
        Edge* isEdgeInGraph(Node* N1,Node* N2);
        
        Node* searchConf(Configuration& q);
        
        void createCompco(Node* node);
        
        Node* insertNode(Node* node);
        
        
        Node* insertExtremalNode(Node* node);
        
        void addNode(Node * node);

        Node* insertNode(Node* expansionNode, LocalPath& path);
        
        Node* insertConfigurationAsNode(std::tr1::shared_ptr<Configuration> q, Node* from, double step );
        
        void sortEdges();
        
    void updateSortedNodes(Node* node);
    Node* getRandomNodeNearTraj();
    void updateSepareNodes(Node* n);
    void separateNodes();
    std::map<double, Node*> getNodesOnTraj(Node* near,bool neighbour = false);
    std::vector<Node*> getKNodesOnTraj(Node* near, uint k);
    void sortNodes(double init = -1);

        Edge * addEdge(Node * source, Node * target, bool compute_length = true, double length = 0.,
                        bool compute_cost = true, double cost = 0.);

        Edge * addEdge(Node * source, Node * target, localPathPtr_t path);

        void addEdges(Node * N1, Node * N2, bool computeLength = true, double length = 0.,
                        bool computeCost = true, double cost = 0.);
        
        void addEdges(Node * N1, Node * N2, localPathPtr_t path);

    void addOrientedEdge(Node * N1, Node * N2, localPathPtr_t path);

        void removeEdge(Edge* E);

        void removeEdges( Node* N1, Node* N2 );
        
        void sortNodesByDist(Node* N);
  
        void sortNodesByDist(confPtr_t q);

        void addNode(Node* N, double maxDist);
        
        void addNodes(std::vector<Node*> N, double maxDist);

        void removeNode(Node * node);

        bool linkNode(Node* N);
  
        bool linkNodeCompMultisol(Node *N, ConnectedComponent* compPt);
        
        bool linkNodeWithoutDist(Node* N);
        
        bool linkNodeAtDist(Node* N);
        
        bool linkToAllNodes(Node* N);
        
        
        bool areNodesLinked(Node* node1, Node* node2, double & dist);
        
        bool linkNodeAndMerge(Node* node1, Node* node2);
        
        void createRandConfs(int NMAX);
        
        Node* randomNodeFromComp(Node* comp);
        
        bool connectNodeToCompco(Node* node, Node* compco);
  
  
        static void sortNodesByDist(std::vector<Node*>& nodes, confPtr_t config);
        
    std::vector<Node*> KNearestWeightNeighbour(confPtr_t q, int K, double radius, bool weighted,
            int distConfigChoice);

        std::vector<Node *> kNearestNeighbours(unsigned k, confPtr_t conf);

        std::vector<Node *> neighborsInBall(confPtr_t conf, double radius);
        
        Node* nearestWeightNeighbour(Node* compco, confPtr_t C, bool weighted, int distConfigChoice);
        
        Node* getCompco(unsigned int ith);
        
        std::vector<Node*> getNodesInTheCompCo(Node* node);
        
        void addCycles(Node* node, double step);

        void mergeComponents(Node * N1, Node * N2);

        /*fonction mergeant les compco N1 et N2*/
        int mergeComp(Node* CompCo1, Node* CompCo2, double DistNodes, bool compute_edge_cost);
        
        int rebuildCompcoFromBoostGraph();
        
        void mergeCheck();
        
        void removeCompco(ConnectedComponent* CompCo);
        
        bool checkConnectedComponents();
        
        void recomputeCost();
        
        bool checkAllEdgesValid();
    
        API::Trajectory* extractBestTrajSoFar(confPtr_t qi, confPtr_t qf);
  
        std::pair<bool, std::vector<Node*> > extractBestNodePathSoFar(confPtr_t qi, confPtr_t qf);

        API::Trajectory * extractTreeShortestPath(Node * initN, Node * endN);

        std::vector<double> computeDijkstraShortestPaths(Node * n);
        std::vector<double> computeDijkstraShortestPaths(Node * n, std::map<double, Node *> to);

        std::vector<Node *> extractAStarShortestNodePaths(Node * initN, Node * endN);
    std::vector<Node *> extractAStarShortestNodePaths(std::vector<Node *> passingBy);

        std::vector<Node *> extractAStarShortestNodePaths(confPtr_t initC, confPtr_t endC);

        API::Trajectory * extractAStarShortestPath(Node * initN, Node * endN);

        API::Trajectory * extractAStarShortestPath(confPtr_t initC, confPtr_t endC);

    double getAStarShortestPathLength(confPtr_t initC, confPtr_t endC);

        API::Trajectory * extractBestTraj(confPtr_t qi, confPtr_t qf);

        void initMotionPlanning(node* start, node* goal);
        
        // BGL functions
        BGL_Vertex findVertexDescriptor(Node* N);
        BGL_Edge findEdgeDescriptor(Edge* E);
        void saveBGLGraphToDotFile(const std::string& filename);
        BGL_Graph& get_BGL_Graph() { return m_BoostGraph; }

        void draw();

    // Used for directed graphs
    void resetConnections();
    void unflagAll();
    unsigned int getNbScc();
    std::vector<std::vector<Node*> > getStronglyConnectedComponents();
    std::vector<double> sortSCC(unsigned int id,Node *N1,bool forward);
    void addScc(Node *node);
    bool areSccConnected(unsigned int i,unsigned int j);
    void connectScc(unsigned int i,unsigned int j);
    bool mergeScc(unsigned int i,unsigned int j);

private:
  
  void init();

        // BGL functions
        void initBGL();
        void unsetDescriptors();
        void drawEdge(BGL_Vertex v1, BGL_Vertex v2);
        void drawNode(BGL_Vertex v);

        void deleteGraphStruct();
        void updateCompcoFromStruct();
        void freeResources();
        static bool compareEdges(Edge* E1, Edge* E2);
        static bool compareNodes(Node* N1, Node* N2);

        // Old p3d graph
        graph* m_Graph;
        Robot* m_Robot;

        // Boost Graph Lib 
        BGL_Graph m_BoostGraph;

        // Flag that is set to false 
        // each time the graph is exported to p3d_graph
        bool m_graphChanged;bool m_initBGL;

        std::vector<Node*> m_Nodes;
    std::vector<Node*> m_SortedNodes;
    std::vector<Node*> m_tempTrajNodes;
    std::vector<Node*> m_TrajNodes;
    std::vector<Node*> m_NotTrajNodes;    
    std::vector<Node*> m_NeighbourNodes;    
    
        std::vector<Edge*> m_Edges;
        std::vector<ConnectedComponent*> m_Comp;

        // Maps between old and new nodes
        std::map<node*, Node*> m_NodesTable;

        // Graph name
        std::string m_Name;

    std::map<Node*,bool> m_UpdateList;

    // list of the strongly connected components
    std::vector<std::vector<Node*> > m_StronglyConnectedComponents;
    // adjacency matrix of the transitive closure of the graph of the strongly connected components
    std::vector<std::vector<bool> > m_Scc_TransitiveClosure;

};

extern Graph* API_activeGraph;

#endif