root/sandbox/orkney/astar.h

/*
 * A* Shortest path algorithm for PostgreSQL
 *
 * Copyright (c) 2007 Anton A. Patrushev, Orkney, Inc.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 */

#ifndef _ASTAR_H_INCLUDED
#define _ASTAR_H_INCLUDED

#include "ogr_core.h"
#include "ogr_feature.h"
#include "ogr_geometry.h"

#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/astar_search.hpp>

#include <math.h>    // for sqrt

#include "dijkstra.h"


using namespace std;
using namespace boost;

// visitor that terminates when we find the goal
template <class Vertex>
class astar_goal_visitor : public boost::default_astar_visitor
{
public:
  astar_goal_visitor(Vertex goal) : m_goal(goal) {}
  template <class Graph>
  void examine_vertex(Vertex u, Graph& g) {
    if(u == m_goal)
      throw found_goal();
  }
private:
  Vertex m_goal;
};

template <class Graph, class CostType>
class distance_heuristic : public astar_heuristic<Graph, CostType>
{
public:
  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  distance_heuristic(Graph& g, Vertex goal):m_g(g), m_goal(goal){}
  CostType operator()(Vertex u)
  {
    CostType dx = m_g[m_goal].x - m_g[u].x;
    CostType dy = m_g[m_goal].y - m_g[u].y;
    //You can choose any heuristical function from below
    //return ::max(dx, dy);
    //return ::sqrt(dx * dx + dy * dy)/4;
    //return 0;
    return (::fabs(dx)+::fabs(dy))/2;
  } 
private:
    Graph& m_g;
    Vertex m_goal;
};

class AStar : public Dijkstra
{
  public:
             AStar(){}
             AStar( char *nameIn, bool directedIn, bool weightedIn, bool reverseCostIn )
             : Dijkstra( nameIn, directedIn, weightedIn, reverseCostIn ){}
    virtual ~AStar(){} 

    bool addEdge(edge_descriptor *e, OGRFeature *edge, graph_t &graph)
    {
      bool inserted;
    
      if (edge->GetFieldAsDouble(COST_FIELD) < 0) // edges are not inserted in the graph if cost is negative
        return false;

      tie(*e, inserted) = add_edge(edge->GetFieldAsInteger(SOURCE_FIELD), 
                                   edge->GetFieldAsInteger(TARGET_FIELD), graph);

      graph[*e].cost = edge->GetFieldAsDouble(COST_FIELD);
      graph[*e].id = edge->GetFieldAsInteger(ID_FIELD);

      OGRLineString *geom = static_cast<OGRLineString*>(edge->GetGeometryRef());

      vertex_descriptor s = vertex(edge->GetFieldAsInteger(SOURCE_FIELD), graph);
      vertex_descriptor t = vertex(edge->GetFieldAsInteger(TARGET_FIELD), graph);

      graph[s].x=geom->getX(0);
      graph[s].y=geom->getY(0);
      graph[t].x=geom->getX(geom->getNumPoints()-1);
      graph[t].y=geom->getY(geom->getNumPoints()-1);
      
      return inserted;
    }


    int getRoute(int start, int end, OGRFeature **edges, OGRFeature **result, int *resultCount, graph_t &graph)
    {
      std::vector<vertex_descriptor> predecessors(num_vertices(graph));

      vertex_descriptor source = vertex(start, graph);

      if (source < 0) 
      {
        return NO_SOURCE_FOUND;
      }

      vertex_descriptor target = vertex(end, graph);
      if (target < 0)
      {
        return NO_TARGET_FOUND;
      }

      std::vector<double> distances(num_vertices(graph));

      try 
      {
        // call astar named parameter interface
        astar_search
          (graph, source,
           distance_heuristic<graph_t, float>(graph, target),
           predecessor_map(&predecessors[0]).
           weight_map(get(&Edge::cost, graph)).
           distance_map(&distances[0]).
           visitor(astar_goal_visitor<vertex_descriptor>(target)));
      } 
      catch(found_goal fg) 
      {
        return makeResult(target, source, edges, predecessors, result, resultCount, graph);
      } 
      return NO_PATH_FOUND; 
    }
}; 

#endif