Boost Graph Problem v2

/*
 *
 */

#include <boost/config.hpp>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include "dijkstra.h"

using namespace std;
using namespace boost;


struct Edge
{
  int id;
  float8 cost;
};

struct Vertex
{
  int id;
  int edge_id;
};

template <class G, class E>
static void
graph_add_edge(G &graph, int id, int source, int target, float8 cost)
{
  //typedef typename graph_traits<G>::edge_descriptor E;
  E e;
  bool inserted;

  if (cost < 0) // edges are not inserted in the graph if cost is negative
    return;

  tie(e, inserted) = add_edge(source, target, graph);

  graph[e].cost = cost;
  graph[e].id = id;

  typedef typename graph_traits<G>::vertex_descriptor Vertex;
  Vertex s = vertex(source, graph);
  Vertex t = vertex(target, graph);
  graph[s].id = source;
  graph[t].id = target;
  graph[s].edge_id = id;
  graph[t].edge_id = id;

}

template <class G, class E>
static int
get_edge_id(G &graph, int source, int target)
{
  //typedef typename graph_traits<G>::edge_descriptor E;
  E e;
  bool found;

  //tie(e, found) = boost::edge(source, target, graph);
  tie(e, found) = edge_t(source, target, graph);
  return graph[e].id;
}

int
boost_dijkstra_nodes(edge_t *edges, unsigned int count, int source_vertex_id,
                    double rdistance, bool directed, bool has_reverse_cost,
                    path_element_t **path, int *path_count, char **err_msg)
{
  typedef adjacency_list < listS, vecS, directedS, Vertex, Edge > graph_t;
  typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
  typedef graph_traits < graph_t >::edge_descriptor edge_descriptor;

  const unsigned int num_nodes = 1;

  graph_t graph( num_nodes );

  property_map<graph_t, edge_weight_t>::type weightmap =
    get(edge_weight, graph);

  for (std::size_t j = 0; j < count; ++j)
    {
      graph_add_edge<graph_t, edge_descriptor>
        (graph, edges[j].id, edges[j].source,
         edges[j].target, edges[j].cost);

      if (!directed || (directed && has_reverse_cost))
        {
          if (has_reverse_cost)
            {
              graph_add_edge<graph_t, edge_descriptor>
                (graph, edges[j].id, edges[j].target,
                 edges[j].source, edges[j].rcost);
            }
          else
            {
              graph_add_edge<graph_t, edge_descriptor>
                (graph, edges[j].id, edges[j].target,
                 edges[j].source, edges[j].cost);
            }
        }
    }

  vertex_descriptor source_vertex = vertex( source_vertex_id, graph );

  std::vector<vertex_descriptor> predecessors(num_vertices(graph));
  std::vector<float8> distances(num_vertices(graph));

  dijkstra_shortest_paths(graph, source_vertex,
                          predecessor_map(&predecessors[0])
                          .weight_map(get(&Edge::cost, graph))
                          .distance_map(&distances[0]));


  graph_traits < graph_t >::vertex_iterator vi, vend;
  vector<path_element> path_vector;
  int j=0;

  for(tie(vi, vend) = vertices(graph); vi != vend; vi++) {

    if( (double)distances[*vi] <= rdistance ) {

      path_element pe;

      graph_traits<graph_t>::vertex_descriptor s;
      graph_traits<graph_t>::vertex_descriptor p;

      s = vertex(*vi, graph);
      p = vertex(predecessors[*vi], graph);

      pe.vertex_id = graph[s].id;
      pe.parent_id = graph[p].id;
      pe.edge_id   = get_edge_id(graph_t, pe.parent_id, pe.vertex_id);
      pe.cost      = distances[*vi];

      path_vector.push_back( pe );
    }
  }

  if( path_vector.size() == 0 ) {
    *err_msg = (char *)"No path found";
    return 0;
  }

  vector<path_element>::iterator itr;
  *path = (path_element_t *) malloc( sizeof(path_element_t) *
                                     (path_vector.size() + 1) );
  *path_count = path_vector.size();

  for(j=0,itr=path_vector.begin();itr != path_vector.end();itr++,j++) {
    path_element pe = *itr;
    (*path)[j].vertex_id = pe.vertex_id;
    (*path)[j].parent_id = pe.parent_id;
    (*path)[j].edge_id   = pe.edge_id;
    (*path)[j].cost      = pe.cost;
  }

  return EXIT_SUCCESS;
}