root/sandbox/orkney/shootingstar.h

/*
 * Shooting* 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 _SHOOTINGSTAR_H_INCLUDED
#define _SHOOTINGSTAR_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/vector_property_map.hpp>

#include <math.h>    // for sqrt and fabs

#include "algorithm.h"
#include "shooting_star_search.hpp"


#define MAX_RULE_LENGTH 5

#define MAX_NODES 1000000
#define MAX_COST  100000

using namespace std;
using namespace boost;

// visitor that terminates when we find the goal
template <class Edge>
class shooting_star_goal_visitor : public boost::default_shooting_star_visitor
{
public:
  shooting_star_goal_visitor(Edge goal, int max_id) : m_goal(goal){}
  shooting_star_goal_visitor(const shooting_star_goal_visitor &gv) : m_goal(gv.m_goal){}
  ~shooting_star_goal_visitor(){}

  template <class Graph>
  void examine_edge(Edge e, Graph& g) 
  {
    if( g[e].id == g[m_goal].id)
    {
      throw found_goal();
    }
  }
  template <class Graph>
  void finish_edge(Edge e, Graph& g) {}
private:
  Edge m_goal;
};

template <class Graph, class CostType>
class edge_distance_heuristic
{
public:
  typedef typename graph_traits<Graph>::edge_descriptor Edge;
  edge_distance_heuristic(Graph& g, Edge goal):m_g(g), m_goal(goal){}
  CostType operator()(Edge e)
  {
    CostType dx = m_g[source(m_goal, m_g)].x - m_g[source(e, m_g)].x;
    CostType dxt = m_g[target(m_goal, m_g)].x - m_g[target(e, m_g)].x;
    CostType dy = m_g[source(m_goal, m_g)].y - m_g[source(e, m_g)].y;
    CostType dyt = m_g[target(m_goal, m_g)].y - m_g[target(e, m_g)].y;
    //You can choose any heuristical function from below
    //return ::max(dx, dy);
    //return ::sqrt(dx * dx + dy * dy)/2;
    //return 0;
    return (min(::fabs(dx),::fabs(dxt))+min(::fabs(dy),::fabs(dyt)))/2;
  } 
private:
  Graph& m_g;
  Edge m_goal;
};

class ShootingStar : public Algorithm
{    
  public:
             ShootingStar():offset(1), rule_num(0){}
             ShootingStar( char *nameIn, bool directedIn, bool weightedIn, bool reverseCostIn )
             : Algorithm( nameIn, directedIn, weightedIn, reverseCostIn ), offset(1), rule_num(0){}
    virtual ~ShootingStar();

  private:
    std::map< int, vector< std::pair<float, vector<int> > >, std::less<int> > adjacent_edges;
    std::map< int, int, std::less<int> > vertices;  
    vector<int> rule;
    int offset;
    int rule_num;

    bool addEdge(edge_descriptor *e, OGRFeature *edge, graph_t &graph)
    {
      bool inserted;
    
      if (edge->GetFieldAsDouble(COST_FIELD) < 0) // edges are inserted as unpassable if cost is negative
        edge->SetField(COST_FIELD, MAX_COST);

      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);

      graph[*e].source = edge->GetFieldAsInteger(SOURCE_FIELD);
      graph[*e].target = edge->GetFieldAsInteger(TARGET_FIELD);

      graph[*e].adjacent_edges = adjacent_edges;

      graph[*e].rank = 0;
      graph[*e].distance = 0;

      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].id = edge->GetFieldAsInteger(SOURCE_FIELD);
      graph[t].id = edge->GetFieldAsInteger(TARGET_FIELD);

      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;
    }

    virtual void fillFeatures(OGRFeature **edges, unsigned int count, edge_descriptor *e, graph_t &graph, int j)
    {
      int src, trg;
      
      //Vertex ids renumbering moved here
      src = edges[j]->GetFieldAsInteger(SOURCE_FIELD);
      trg = edges[j]->GetFieldAsInteger(TARGET_FIELD);
    
      if(vertices[src]==0)
      {
        vertices[src]=j+offset;
      }
      edges[j]->SetField(SOURCE_FIELD, vertices[src]);
    
      if(vertices[trg]==0)
      {
        offset++;      
        vertices[trg]=j+offset;
      }
      edges[j]->SetField(TARGET_FIELD, vertices[trg]);
      
      int *rule_cnt;
      const int *rule_list;
      
      rule_list = edges[j]->GetFieldAsIntegerList(RULE_FIELD, rule_cnt);
    
      for(int z=0; z< *rule_cnt;++z)
      {
        if(rule_list[z] > 0)
        {
          rule.push_back(rule_list[z]);
        }
      }

      if(edges[j]->GetFieldAsDouble(TO_COST_FIELD) > 0)
      {
        adjacent_edges[edges[j]->GetFieldAsInteger(ID_FIELD)].push_back(
                                std::pair<double, vector<int> > (edges[j]->GetFieldAsDouble(TO_COST_FIELD), rule) );
        rule.clear();
      }

      if((j < count-1 && edges[j]->GetFieldAsInteger(ID_FIELD) != edges[j+1]->GetFieldAsInteger(ID_FIELD))||(j==count-1))
      {

        bool inserted = addEdge(e, edges[j], graph);

        graph[*e].cost = edges[j]->GetFieldAsDouble(COST_FIELD);//set cost
        graph[*e].id = j;//set id


        if (!IsDirected() || (IsDirected() && HasReverseCost()))
        {

          if(adjacent_edges[edges[j]->GetFieldAsInteger(ID_FIELD)].size() > 0)
          {
            adjacent_edges[edges[j]->GetFieldAsInteger(ID_FIELD)+e_max_id].assign( 
                              adjacent_edges[edges[j]->GetFieldAsInteger(ID_FIELD)].begin(), 
                              adjacent_edges[edges[j]->GetFieldAsInteger(ID_FIELD)].end() );
                adjacent_edges.erase(edges[j]->GetFieldAsInteger(ID_FIELD));
          }

          edge_descriptor *er; 
          //adding an edge for opposite direction
              bool inserted = addEdge(er, edges[j], graph);

          graph[*er].id = j;//set id
    
          if (HasReverseCost())
              {
            graph[*er].cost = edges[j]->GetFieldAsDouble(RC_FIELD);//set reverse cost
              }
              else 
              {
                graph[*er].cost = edges[j]->GetFieldAsDouble(COST_FIELD);//set cost
              }

        }  

        adjacent_edges.clear();
        rule_num = 0;
        
      }
      else
      {
        rule_num++;
      }   
    }

    int getRoute(int start, int end, OGRFeature **edges, OGRFeature **result, int *resultCount, graph_t &graph)
    {      
      edge_descriptor source_edge;
      edge_descriptor target_edge;
  
      bool source_found = false, target_found = false;      

      graph_traits<graph_t>::edge_iterator ei, ei_end;

      for(tie(ei, ei_end) = boost::edges(graph); ei != ei_end; ++ei) 
      {
        if(graph[*ei].id == start)
        {
          source_edge = *ei;
          source_found = true;
          break;
        }
      }

      if (!source_found) 
      {
        return NO_SOURCE_FOUND;
      }


      for(tie(ei, ei_end) = boost::edges(graph); ei != ei_end; ++ei) 
      {
        if(graph[*ei].id == end)
        {
          target_edge = *ei;
          target_found = true;
          break;
        }
      }

      if (!target_found)
      {
        return NO_TARGET_FOUND;
      }

      property_map<graph_t, int Edge::*>::type edge_index = get(&Edge::id, graph);

      std::map< int, edge_descriptor, std::less<int> > predecessors;
  
      property_map<graph_t, double Edge::*>::type rank = get(&Edge::rank, graph);
      property_map<graph_t, double Edge::*>::type distance = get(&Edge::distance, graph);
  
      try 
      {
        shooting_star_search
          (graph, source_edge,
           edge_distance_heuristic<graph_t, float>(graph, target_edge),
           weight_map(get(&Edge::cost, graph)).
           weight_map2(get(&Edge::adjacent_edges, graph)).
           edge_color_map(get(&Edge::color, graph)).
           visitor(shooting_star_goal_visitor<edge_descriptor>(target_edge, e_max_id)),
           edge_index,
           distance, rank,
           predecessors, e_max_id
           );
      } 
      catch(found_goal &fg) 
      {
        return makeResult(target_edge, source_edge, edges, predecessors, result, resultCount, graph);
      }  
    }
    
    int  makeResult(edge_descriptor target_edge, edge_descriptor source_edge, 
                            OGRFeature **edges,
                            std::map< int, edge_descriptor, std::less<int> > &predecessors, 
                            OGRFeature **result, int *resultCount, graph_t &graph)
    {
    vector<edge_descriptor> path_vect;
    int max = MAX_NODES;

    path_vect.push_back(target_edge);
    
    while (target_edge != source_edge) 
      {
        
        if ((target_edge == predecessors[graph[target_edge].id]) && (predecessors[graph[target_edge].id] != source_edge))
            {
          return NO_PATH_FOUND;    
            }
  
            target_edge = predecessors[graph[target_edge].id];

        path_vect.push_back(target_edge);

        if (!max--) 
            {
           return RESULT_OVERFLOW;
            }
      }

      //*result = (OGRFeature *) malloc(sizeof(OGRFeature) * (path_vect.size() + 1));
      //Let's try to do it in C++ style
      result = new OGRFeature * [path_vect.size() + 1];
      *resultCount = path_vect.size();

      for(int i = path_vect.size() - 1, j = 0; i >= 0; i--, j++)
      {
        graph_traits < graph_t >::edge_descriptor e;

        e = path_vect.at(i);

        if(graph[e].id > e_max_id)
        {
          graph[e].id -= e_max_id;      
        }
      
        result[j] = edges[graph[e].id];
      }

      return EXIT_SUCCESS;
    }
    
}; 

#endif