kraken: Fixing undefined behaviour and crash of A*

source/autocomplete/autocomplete.cpp

@@ -30,6 +30,9 @@ www.navitia.io
 
 #include "autocomplete.h"
 #include "type/pt_data.h"
+#include "type/stop_area.h"
+#include "type/stop_point.h"
+
 namespace navitia {
 namespace autocomplete {
 

source/autocomplete/autocomplete.h

@@ -40,7 +40,9 @@ www.navitia.io
 #include <map>
 #include <unordered_map>
 #include <set>
-#include "type/type.h"
+#include "type/type_interfaces.h"
+#include "type/geographical_coord.h"
+#include "type/fwd_type.h"
 #include "utils/functions.h"
 
 namespace navitia {

source/cities/cities.cpp

@@ -43,7 +43,7 @@ www.navitia.io
 #include "ed/connectors/osm_tags_reader.h"
 #include "utils/functions.h"
 #include "utils/init.h"
-
+#include <iomanip>
 #include "conf.h"
 
 namespace po = boost::program_options;
@@ -52,6 +52,21 @@ namespace pt = boost::posix_time;
 namespace navitia {
 namespace cities {
 
+std::string OSMNode::coord_to_string() const {
+    std::stringstream geog;
+    geog << std::setprecision(10) << lon() << " " << lat();
+    return geog.str();
+}
+
+std::string OSMWay::coord_to_string() const {
+    std::stringstream geog;
+    geog << std::setprecision(10);
+    for (auto node : nodes) {
+        geog << node->second.coord_to_string();
+    }
+    return geog.str();
+}
+
 /*
  * Read relations
  * Stores admins relations and initializes nodes and ways associated to it.

source/cities/cities.h

@@ -30,15 +30,14 @@ www.navitia.io
 
 #pragma once
 #include <set>
-#include "type/type.h"
 #include <osmpbfreader/osmpbfreader.h>
 #include "utils/lotus.h"
 #include "utils/logger.h"
 #include <unordered_map>
-#include "ed/types.h"
 #include <boost/geometry/geometries/point.hpp>
 #include <boost/geometry/geometries/polygon.hpp>
 #include <boost/geometry/multi/geometries/multi_polygon.hpp>
+#include <boost/geometry/core/cs.hpp>
 #include <RTree/RTree.h>
 #include <boost/algorithm/string.hpp>
 
@@ -101,11 +100,7 @@ struct OSMNode {
         return std::abs(this->ilon - other.ilon) < distance && std::abs(this->ilat - other.ilat) < distance;
     }
 
-    std::string coord_to_string() const {
-        std::stringstream geog;
-        geog << std::setprecision(10) << lon() << " " << lat();
-        return geog.str();
-    }
+    std::string coord_to_string() const;
     std::string to_geographic_point() const;
 };
 
@@ -139,14 +134,7 @@ struct OSMWay {
 
     void add_node(std::unordered_map<OSMId, OSMNode>::const_iterator node) { nodes.push_back(node); }
 
-    std::string coord_to_string() const {
-        std::stringstream geog;
-        geog << std::setprecision(10);
-        for (auto node : nodes) {
-            geog << node->second.coord_to_string();
-        }
-        return geog.str();
-    }
+    std::string coord_to_string() const;
 };
 
 typedef std::set<OSMWay>::const_iterator it_way;

source/ed/build_helper.h

@@ -32,6 +32,7 @@ www.navitia.io
 
 #include "utils/functions.h"
 #include "type/data.h"
+#include "type/pt_data.h"
 #include <boost/date_time/gregorian_calendar.hpp>
 #include "georef/georef.h"
 #include "type/meta_data.h"

source/ed/connectors/conv_coord.cpp

@@ -29,6 +29,7 @@ www.navitia.io
 */
 
 #include "conv_coord.h"
+#include "utils/exception.h"
 
 namespace ed {
 namespace connectors {

source/ed/connectors/conv_coord.h

@@ -30,7 +30,7 @@ www.navitia.io
 
 #pragma once
 #include <proj_api.h>
-#include "type/type.h"
+#include "type/geographical_coord.h"
 
 namespace ed {
 namespace connectors {

source/ed/connectors/osm_tags_reader.cpp

@@ -33,6 +33,7 @@ www.navitia.io
 #include <iostream>
 #include <algorithm>
 #include <boost/property_tree/json_parser.hpp>
+#include "utils/exception.h"
 
 namespace ed {
 namespace connectors {

source/ed/connectors/projection_system_reader.cpp

@@ -30,6 +30,7 @@ www.navitia.io
 
 #include "projection_system_reader.h"
 #include <boost/filesystem.hpp>
+#include "utils/exception.h"
 #include "utils/csv.h"
 
 namespace ed {

source/ed/types.h

@@ -35,9 +35,15 @@ www.navitia.io
 #include <boost/date_time/gregorian/greg_serialize.hpp>
 #include <boost/date_time/posix_time/posix_time.hpp>
 #include <boost/date_time/local_time/local_time.hpp>
+#include <boost/optional.hpp>
 #include "type/datetime.h"
 
-#include "type/type.h"
+#include "type/type_interfaces.h"
+#include "type/connection.h"
+#include "type/validity_pattern.h"
+#include "type/geographical_coord.h"
+#include "type/vehicle_journey.h"
+#include "type/calendar.h"
 #include "limits.h"
 
 namespace nt = navitia::type;

source/georef/CMakeLists.txt

@@ -5,6 +5,12 @@ SET(GEOREF_SRC
     street_network.cpp
     adminref.h
     adminref.cpp
+    path_finder.h
+    path_finder.cpp
+    dijkstra_path_finder.h
+    dijkstra_path_finder.cpp
+    astar_path_finder.h
+    astar_path_finder.cpp
 )
 
 add_library(georef ${GEOREF_SRC})

source/georef/adminref.h

@@ -29,7 +29,9 @@ www.navitia.io
 */
 
 #pragma once
-#include "type/type.h"
+#include "type/fwd_type.h"
+#include "type/geographical_coord.h"
+#include "type/type_interfaces.h"
 
 #include <boost/geometry/geometries/polygon.hpp>
 #include <unordered_map>

source/georef/astar_path_finder.cpp

@@ -0,0 +1,130 @@
+/* Copyright © 2001-2014, Canal TP and/or its affiliates. All rights reserved.
+
+This file is part of Navitia,
+    the software to build cool stuff with public transport.
+
+Hope you'll enjoy and contribute to this project,
+    powered by Canal TP (www.canaltp.fr).
+Help us simplify mobility and open public transport:
+    a non ending quest to the responsive locomotion way of traveling!
+
+LICENCE: This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU Affero General Public License as published by
+the Free Software Foundation, either version 3 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 Affero General Public License for more details.
+
+You should have received a copy of the GNU Affero General Public License
+along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+Stay tuned using
+twitter @navitia
+IRC #navitia on freenode
+https://groups.google.com/d/forum/navitia
+www.navitia.io
+*/
+
+#include "astar_path_finder.h"
+#include "visitor.h"
+
+#include <boost/graph/astar_search.hpp>
+#include <boost/graph/filtered_graph.hpp>
+
+namespace navitia {
+namespace georef {
+
+AstarPathFinder::~AstarPathFinder() = default;
+
+void AstarPathFinder::init(const type::GeographicalCoord& start_coord,
+                           const type::GeographicalCoord& dest_projected_coord,
+                           nt::Mode_e mode,
+                           const float speed_factor) {
+    PathFinder::init_start(start_coord, mode, speed_factor);
+
+    // we initialize the costs to the maximum value
+    size_t n = boost::num_vertices(geo_ref.graph);
+    costs.assign(n, bt::pos_infin);
+
+    if (starting_edge.found) {
+        costs.at(starting_edge[source_e]) =
+            compute_cost_from_starting_edge_to_dist(starting_edge[source_e], dest_projected_coord);
+        costs.at(starting_edge[target_e]) =
+            compute_cost_from_starting_edge_to_dist(starting_edge[target_e], dest_projected_coord);
+    }
+}
+
+void AstarPathFinder::start_distance_or_target_astar(const navitia::time_duration& radius,
+                                                     const type::GeographicalCoord& dest_projected,
+                                                     const std::vector<vertex_t>& destinations) {
+    if (!starting_edge.found)
+        return;
+    computation_launch = true;
+    // We start astar from source and target nodes
+    try {
+        astar({starting_edge[source_e], starting_edge[target_e]},
+              astar_distance_heuristic(geo_ref.graph, dest_projected, 1. / double(default_speed[mode] * speed_factor)),
+              astar_distance_or_target_visitor(radius, distances, destinations));
+    } catch (DestinationFound&) {
+    }
+}
+
+/**
+ * Launch an astar without initializing the data structure
+ * Warning, it modifies the distances and the predecessors
+ **/
+void AstarPathFinder::astar(const std::array<georef::vertex_t, 2>& origin_vertexes,
+                            const astar_distance_heuristic& heuristic,
+                            const astar_distance_or_target_visitor& visitor) {
+    // Note: the predecessors have been updated in init
+
+    // Fill color map in white before A*
+    std::fill(color.data.get(), color.data.get() + (color.n + color.elements_per_char - 1) / color.elements_per_char,
+              0);
+
+    // we filter the graph to only use certain mean of transport
+    using filtered_graph = boost::filtered_graph<georef::Graph, boost::keep_all, TransportationModeFilter>;
+    auto g = filtered_graph(geo_ref.graph, {}, TransportationModeFilter(mode, geo_ref));
+    auto weight_map = boost::get(&Edge::duration, geo_ref.graph);
+    auto combiner = SpeedDistanceCombiner(speed_factor);
+
+    astar_shortest_paths_no_init_with_heap(g, origin_vertexes.front(), origin_vertexes.back(), heuristic, visitor,
+                                           weight_map, combiner);
+}
+
+template <class Graph, class WeightMap, class Compare>
+void AstarPathFinder::astar_shortest_paths_no_init_with_heap(const Graph& g,
+                                                             const vertex_t& s_begin,
+                                                             const vertex_t& s_end,
+                                                             const astar_distance_heuristic& h,
+                                                             const astar_distance_or_target_visitor& vis,
+                                                             const WeightMap& weight,
+                                                             const SpeedDistanceCombiner& combine,
+                                                             const Compare& compare) {
+    typedef boost::d_ary_heap_indirect<vertex_t, 4, vertex_t*, navitia::time_duration*, Compare> MutableQueue;
+    MutableQueue Q(&costs[0], &index_in_heap_map[0], compare);
+
+    boost::detail::astar_bfs_visitor<astar_distance_heuristic, astar_distance_or_target_visitor, MutableQueue,
+                                     vertex_t*, navitia::time_duration*, navitia::time_duration*, WeightMap,
+                                     boost::two_bit_color_map<>, SpeedDistanceCombiner, Compare>
+        bfs_vis(h, vis, Q, &predecessors[0], &costs[0], &distances[0], weight, color, combine, compare,
+                navitia::seconds(0));
+
+    breadth_first_visit(g, &s_begin, &s_end, Q, bfs_vis, color);
+}
+
+// The cost of a starting edge is the distance from this edge to the projected destination point (distance_to_dest)
+// Plus the distance from this edge to the projected starting point (distances[v])
+navitia::time_duration AstarPathFinder::compute_cost_from_starting_edge_to_dist(
+    const vertex_t& v,
+    const type::GeographicalCoord& dest_coord) const {
+    auto const& edge_coords = geo_ref.graph[v].coord;
+    auto const distance_to_dest = edge_coords.distance_to(dest_coord);
+    return navitia::seconds(distance_to_dest / double(default_speed[mode] * speed_factor)) + distances[v];
+}
+
+}  // namespace georef
+}  // namespace navitia