Graphs.hpp

Go to the documentation of this file.

#ifndef LGL_GRAPHS_HPP
#define LGL_GRAPHS_HPP

#include <map>
#include <cmath>
#include <vector>

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/iteration_macros.hpp>

#include <lgl/Location.hpp>

using namespace boost;

namespace jafar {
  namespace lgl {
    
    template 
    <
      typename EDGE_BUNDLE,
      typename VERTEX_BUNDLE,
      typename EDGE_WEIGHT_T
      >   
    class NavGraphT {//{{{
      
    public :
      
      // declaration of the directed graph type
      typedef adjacency_list<
      listS,     // out edge container => make search algo faster
      vecS,      // vertex container
      directedS, //directed graph
      VERTEX_BUNDLE, 
      EDGE_BUNDLE
      > GraphContainer;
      
      /* a bunch of graph-specific typedefs */
      typedef typename graph_traits<GraphContainer>::vertex_descriptor Vertex;
      typedef typename graph_traits<GraphContainer>::edge_descriptor Edge;
      typedef std::pair<Edge, Edge> EdgePair;
      
      typedef typename graph_traits<GraphContainer>::vertex_iterator vertex_iterator;
      typedef typename graph_traits<GraphContainer>::edge_iterator edge_iterator;
      typedef typename graph_traits<GraphContainer>::adjacency_iterator adjacency_iterator;
      typedef typename graph_traits<GraphContainer>::out_edge_iterator out_edge_iterator;
      
      typedef typename graph_traits<GraphContainer>::degree_size_type degree_t;
      
      typedef std::pair<adjacency_iterator, adjacency_iterator> adjacency_vertex_range_t;
      typedef std::pair<out_edge_iterator, out_edge_iterator> out_edge_range_t;
      typedef std::pair<vertex_iterator, vertex_iterator> vertex_range_t;
      typedef std::pair<edge_iterator, edge_iterator> edge_range_t;
      
      // map between unique int key and vertex descriptor
      typedef std::map<int,Vertex> PosToVertex;
      // typename is here mandatory
      typedef typename std::map<int,Vertex>::iterator PosToVertex_iterator;

      // the weight map type on which searching algorithm is working
      typedef typename property_map<GraphContainer, EDGE_WEIGHT_T EDGE_BUNDLE::*>::type WeightMap;
      
      GraphContainer graph;
     
      PosToVertex verticesMap;

      int xsize, ysize;

      NavGraphT(int _xsize, int _ysize):
        xsize(_xsize),
        ysize(_ysize)
      {
      }
      
      ~NavGraphT() {}

      Vertex getVertex(const NavLocation& loc) {
        int posId = loc.pos_y * xsize + loc.pos_x;
        return verticesMap[posId];
      }

      bool getVertex(const NavLocation& loc, Vertex& v, bool check=false) {
        if (check==true) {
          if (!(vertexExists(loc)))
          {
            return false;
          }
        }
        v = getVertex(loc);
        return true;
      }
      
      NavLocation getClosestLocation(const NavLocation& _loc) {
        if (vertexExists(_loc))
        {
          return _loc;
        }

        NavLocation foundLoc(0,0);
        double dist = -1.0;
        double dist_tmp = -1.0;
        double xd = 0;
        double yd = 0;
        //Vertex v;
        BGL_FORALL_VERTICES(v,graph, adjacency_list<>) {
          xd = (double)(_loc.pos_x - ((graph)[v]).pos.pos_x);
          yd = (double)(_loc.pos_y - (graph)[v].pos.pos_y);
          dist_tmp = sqrt(xd*xd + yd*yd);

          // Getting closest location || init.
          if ( (dist_tmp < dist) || (dist < 0.0))
          {
            // Saving location
            foundLoc.setPos((graph)[v].pos.pos_x, (graph)[v].pos.pos_y);
            // Smallest distance
            dist = dist_tmp;
          }
        }

        return foundLoc;
      }

      Edge getEdge(const NavLocation& loc1,const NavLocation& loc2) {
        return edge(getVertex(loc1),getVertex(loc2),graph).first;
      }
      
      Edge getEdge(const NavLocation& loc1, const Vertex& vloc2) {
        return edge(getVertex(loc1), vloc2, graph).first;
      }

      Edge getEdge(const Vertex& vloc1,const NavLocation& loc2) {
        return edge(vloc1,getVertex(loc2),graph).first;
      }

      Edge getEdge(const Vertex& vloc1,  const Vertex& vloc2) {
        return edge(vloc1, vloc2, graph).first;
      }
      
      bool vertexExists(const NavLocation& loc) {
        int posId = loc.pos_y * xsize + loc.pos_x;
        PosToVertex_iterator it;
        it = verticesMap.find(posId);       
        if (it==verticesMap.end())
          return false;
        else
          return true;
      }
      
      Vertex addVertex(const NavLocation& loc) {
        int posId = loc.pos_y * xsize + loc.pos_x;
        if (vertexExists(posId))
        {
          return verticesMap[posId];
        } else {
          Vertex v = add_vertex(graph);
          verticesMap.insert(std::pair<int,Vertex>(posId,v));
          // temporary .. set the pos information of the graph vertex
          graph[v].pos = loc;
          return v;
        }
      }
      
      void removeGraphContent() { // DO NOT USE FOR NOW
        PosToVertex_iterator vi, next;
        vi = verticesMap.begin();
        for (next = vi; vi != verticesMap.end(); vi = next)
        {
          ++next;
          clear_vertex(vi->second, graph);
          remove_vertex(vi->second, graph);
        }
        verticesMap.clear();
      }

      bool removeVertex(const NavLocation& loc) {
        int posId = loc.pos_y * xsize + loc.pos_x;
        if (!vertexExists(posId))
          return true;
        else {
          Vertex v = verticesMap[posId];
          // delete in the graph container
          //  .. by firstly removing all in and out edges of v with clear_vertex() boost method
          clear_vertex(v,graph);
          remove_vertex(v,graph);
          // delete in the map
          verticesMap.erase(posId);
          return true;
        }
      }

      bool removeEdgesOnly(const NavLocation& loc) {
        int posId = loc.pos_y * xsize + loc.pos_x;
        if (!vertexExists(posId))
        {
          return false;
        }
        // else
        Vertex v = verticesMap[posId];
        // use clear_vertex of boost, this method clear all the edges connected to the given vertex 
        // but still let the vertex in the graph
        clear_vertex(v,graph);
        return true;        
      }
      
      bool removeOutEdges(const NavLocation& loc) {
        int posId = loc.pos_y * xsize + loc.pos_x;
        if (!vertexExists(posId))
        {
          return false;
        }

        // Removing out edges
        Vertex v = verticesMap[posId];
        out_edge_iterator oei, edge_end;
        tie(oei,edge_end) = out_edges(v,graph); 
        for (; oei != edge_end; ++oei) {
          // oei instead of *oei for speeding up the operation
          remove_edge(oei,graph);
        }

        return true;
      }

      bool removeInEdges(const NavLocation& loc) {
        int posId = loc.pos_y * xsize + loc.pos_x;
        if (!vertexExists(posId))
        {
          return false;
        }

        // Removing in-edges
        Vertex v = verticesMap[posId];
        adjacency_iterator ai, adj_end;
        tie(ai,adj_end) = adjacent_vertices(v,graph);
        for (; ai != adj_end; ++ai) {
          remove_edge(*ai,v,graph);
        }
        return true;
      }

      bool addEdge(const NavLocation& loc1, const NavLocation& loc2, bool check=false) {
        Vertex v1,v2;
        if (check) {
          v1 = addVertex(loc1);
          v2 = addVertex(loc2);
        } else {          
          v1 = getVertex(loc1);
          v2 = getVertex(loc2);
        }

        // Add edge to the graph
        add_edge(v1,v2,graph);
        
        // TODO: check if (true) the edge is newly added or (false) that it
        return true;
      }

      bool addEdge(const NavLocation& loc1, const NavLocation& loc2, Edge& edge, bool check=false) {
        Vertex v1,v2;
        if (check) {
          v1=addVertex(loc1);
          v2=addVertex(loc2);
        } else {          
          v1 = getVertex(loc1);
          v2 = getVertex(loc2);
        }

        // Save edge description
        edge = add_edge(v1,v2,graph).first;
        
        return true;
      }
      
      bool addEdge(const NavLocation& loc1, const NavLocation& loc2, const EDGE_BUNDLE& bundle, bool check=false) {
        Vertex v1,v2;
        if (check) {
          v1=addVertex(loc1);
          v2=addVertex(loc2);
        } else {          
          v1 = getVertex(loc1);
          v2 = getVertex(loc2);
        }
        Edge edge = add_edge(v1,v2,graph).first;
        
        setEdgeBundle(edge,bundle);

        return true;
      }

      bool addDualEdge(const NavLocation& loc1, const NavLocation& loc2)
      {
        // Creating vertices and checking if they don't already exist
        // anyway returning the corresponding vertex
        Vertex v1 = addVertex(loc1);
        Vertex v2 = addVertex(loc2);
        
        add_edge(v1,v2,graph);
        add_edge(v2,v1,graph);

        return true;
      }

      bool addDualEdgeFast(const NavLocation& loc1, const NavLocation& loc2)
      {
        
        Vertex v1 = getVertex(loc1);
        Vertex v2 = getVertex(loc2);
        
        add_edge(v1,v2,graph);
        add_edge(v2,v1,graph);
        
        return true;
      }
            
      bool addDualEdgeFast(const NavLocation& loc1, const NavLocation& loc2, const EDGE_BUNDLE& edge1, const EDGE_BUNDLE& edge2)
      {
        
        Vertex v1 = getVertex(loc1);
        Vertex v2 = getVertex(loc2);
        
        add_edge(v1,v2,graph);
        add_edge(v2,v1,graph);
        
        return true;
      }


      // add edges between all vertices
      bool mesh(std::vector<NavLocation>& locs) {
        int nbLocs = locs.size();
        int i,j;
        for (i=0;i<nbLocs;i++) {
          addVertex(locs[i]);
        }       
        for (i=0;i<nbLocs;i++) {
          for (j=i+1;j<nbLocs;j++) {
            addDualEdgeFast(locs[i],locs[j]);
          }
        }
        return true;
      }

      // add edges between all In Out locations
      // dont connect two postions with same x or same y
      bool connectInOutLocs(std::vector<NavLocation>& locs) {
        int nbLocs = locs.size();
        int i,j;
        for (i=0;i<nbLocs;i++) {
          addVertex(locs[i]);
        }       
        for (i=0;i<nbLocs;i++) {
          for (j=i+1;j<nbLocs;j++) {
            if ((locs[i].pos_x!=locs[j].pos_x)&&(locs[i].pos_y!=locs[j].pos_y))
              addDualEdgeFast(locs[i],locs[j]);
          }
        }
        return true;
      }

      // add edges between all vertices
      bool connect(NavLocation locToConnect, std::vector<NavLocation>& locs, bool verbose=false) {
        // Connecting it to its given neighborhood
        int nbLocs = locs.size();       
        for (int i=0;i<nbLocs;i++) {
          if (verbose)
          {
            std::cout << "# Connect " << locToConnect << " to " << locs[i] << std::endl;
          }
          // in the mean time adds mission locations
          addDualEdge(locToConnect,locs[i]);
        }       
        return true;
      }

      bool removeEdge(const NavLocation& loc1, const NavLocation& loc2) {
        if ((!vertexExists(loc1))||(!vertexExists(loc2)))
        {
          return false;
        }

        Vertex v1 = getVertex(loc1);
        Vertex v2 = getVertex(loc2);

        remove_edge(v1,v2,graph);
        return true;
      }

      bool removeDualEdge(const NavLocation& loc1, const NavLocation& loc2) {
        
        if ((!vertexExists(loc1))||(!vertexExists(loc2)))
        {
          return false;
        }
        
        Vertex v1 = getVertex(loc1);
        Vertex v2 = getVertex(loc2);

        remove_edge(v1,v2,graph);
        remove_edge(v2,v1,graph);
        return true;
      }

      bool setVertexBundle(const NavLocation& loc, const VERTEX_BUNDLE& bundle) {               
        
        Vertex v;
        if (!vertexExists(loc))
            v = addVertex(loc);
        else
          v = getVertex(loc);
        
        // set the bundle to the vertex
        get(vertex_bundle, graph)[v]=bundle;
        
        return true;
      }

      bool setEdgeBundle(const Edge& e, const EDGE_BUNDLE& bundle) {
        get(edge_bundle, graph)[e] = bundle;
        return true;
      }

      bool setEdgeBundle(const Vertex& v1, const Vertex& v2, const EDGE_BUNDLE& bundle) {
        get(edge_bundle, graph)[edge(v1,v2,graph).first]=bundle;
        return true;
      }

      bool setEdgeBundle(const NavLocation& loc1, const NavLocation& loc2, const EDGE_BUNDLE& bundle, bool check=false) {               
        if (check)
        {
          if ((!vertexExists(loc1)) || (!vertexExists(loc2)))
          {
            return false;
          }
        }
        Vertex v1 = getVertex(loc1);
        Vertex v2 = getVertex(loc2);
        
        get(edge_bundle, graph)[edge(v1,v2,graph).first]=bundle;
        
        return true;
      }
      
      bool getVertexBundle(const NavLocation& loc, VERTEX_BUNDLE& bundle, bool check=false) {       
        if (check)
          if (!vertexExists(loc))
            return false;
        Vertex v = getVertex(loc);
        
        // get the bundle from the vertex
        bundle = get(vertex_bundle, graph)[v];
        return true;
      }

      bool getVertexBundle(const Vertex& v, VERTEX_BUNDLE& bundle) {        
        // get the bundle from the vertex
        bundle = get(vertex_bundle, graph)[v];
        return true;
      }

      bool getEdgeBundle(const Edge& e, EDGE_BUNDLE& bundle) {//{{{
        bundle = get(edge_bundle, graph)[e];
        return true;
      }//}}}

      bool getEdgeBundle(const Vertex& v1, const Vertex& v2, EDGE_BUNDLE& bundle) {//{{{  
        bundle = get(edge_bundle, graph)[edge(v1,v2,graph).first];
        return true;
      }//}}}
      
      bool getEdgeBundle(const NavLocation& loc1, const NavLocation& loc2, EDGE_BUNDLE& bundle, bool check=false) {//{{{
        if (check)
        {
          if ((!vertexExists(loc1)) || (!vertexExists(loc2)))
          {
            return false;
          }
        }
        Vertex v1 = getVertex(loc1);
        Vertex v2 = getVertex(loc2);
        
        bundle = get(edge_bundle, graph)[edge(v1,v2,graph).first];
        return true;
      }//}}}

      GraphContainer* getGCtn() {
        return (&graph);
      }
      
      void clear() {
        graph.clear();
        verticesMap.clear();
      }


      /* operators */
      NavGraphT& operator=(const NavGraphT &rhs)
      {
        graph = rhs.graph;
        return *this;
      }
      
      void print() {

        vertex_iterator i, end;
        out_edge_iterator ei, edge_end;
        for(tie(i,end) = vertices(graph); i != end; ++i) {
          for (tie(ei,edge_end) = out_edges(*i, graph); ei != edge_end; ++ei) {
            std::cout << graph[*i].pos.pos_x <<":" << graph[*i].pos.pos_y; 
            std::cout << " --> ";
            std::cout << graph[target(*ei, graph)].pos.pos_x <<":" << graph[target(*ei, graph)].pos.pos_y;
            std::cout << std::endl;
          }

        }
      }

    private:
      
      bool vertexExists(const int& posId) {
        PosToVertex_iterator it;
        it = verticesMap.find(posId);       
        if (it==verticesMap.end())
          return false;
        else
          return true;
        return true;
      }
      

    };//}}}

  }

}

#endif // LGL_GRAPHS_HPP