Implement exclude flags on CH using shared core

The core is fully contracted for each exclude flag
and stored in a merged graph data structure.

include/util/dynamic_graph.hpp

@@ -33,6 +33,27 @@ template <typename EdgeDataT, bool UseSharedMemory>
 void write(storage::io::FileWriter &writer, const DynamicGraph<EdgeDataT> &graph);
 }
 
+namespace detail
+{
+// These types need to live outside of DynamicGraph
+// to be not dependable. We need this for transforming graphs
+// with different data.
+
+template <typename EdgeIterator> struct DynamicNode
+{
+    // index of the first edge
+    EdgeIterator first_edge;
+    // amount of edges
+    unsigned edges;
+};
+
+template <typename NodeIterator, typename EdgeDataT> struct DynamicEdge
+{
+    NodeIterator target;
+    EdgeDataT data;
+};
+}
+
 template <typename EdgeDataT> class DynamicGraph
 {
   public:
@@ -41,6 +62,11 @@ template <typename EdgeDataT> class DynamicGraph
     using EdgeIterator = std::uint32_t;
     using EdgeRange = range<EdgeIterator>;
 
+    using Node = detail::DynamicNode<EdgeIterator>;
+    using Edge = detail::DynamicEdge<NodeIterator, EdgeDataT>;
+
+    template <typename E> friend class DynamicGraph;
+
     class InputEdge
     {
       public:
@@ -120,6 +146,9 @@ template <typename EdgeDataT> class DynamicGraph
         }
     }
 
+    // Copy&move for the same data
+    //
+
     DynamicGraph(const DynamicGraph &other)
     {
         number_of_nodes = other.number_of_nodes;
@@ -130,7 +159,7 @@ template <typename EdgeDataT> class DynamicGraph
         edge_list = other.edge_list;
     }
 
-    DynamicGraph&operator=(const DynamicGraph &other)
+    DynamicGraph &operator=(const DynamicGraph &other)
     {
         auto copy_other = other;
         *this = std::move(other);
@@ -159,6 +188,38 @@ template <typename EdgeDataT> class DynamicGraph
         return *this;
     }
 
+    // Removes all edges to and from nodes for which filter(node_id) returns false
+    template <typename Pred> auto Filter(Pred filter) const &
+    {
+        DynamicGraph other;
+
+        other.number_of_nodes = number_of_nodes;
+        other.number_of_edges = static_cast<std::uint32_t>(number_of_edges);
+        other.edge_list.reserve(edge_list.size());
+        other.node_array.resize(node_array.size());
+
+        NodeID node_id = 0;
+        std::transform(
+            node_array.begin(), node_array.end(), other.node_array.begin(), [&](const Node &node) {
+                const EdgeIterator first_edge = other.edge_list.size();
+                if (filter(node_id++))
+                {
+                    std::copy_if(edge_list.begin() + node.first_edge,
+                                 edge_list.begin() + node.first_edge + node.edges,
+                                 std::back_inserter(other.edge_list),
+                                 [&](const auto &edge) { return filter(edge.target); });
+                    const unsigned num_edges = other.edge_list.size() - first_edge;
+                    return Node{first_edge, num_edges};
+                }
+                else
+                {
+                    return Node{first_edge, 0};
+                }
+            });
+
+        return other;
+    }
+
     unsigned GetNumberOfNodes() const { return number_of_nodes; }
 
     unsigned GetNumberOfEdges() const { return number_of_edges; }
@@ -366,6 +427,7 @@ template <typename EdgeDataT> class DynamicGraph
             for (auto edge : GetAdjacentEdgeRange(node))
             {
                 edge_list[edge].target = old_to_new_node[edge_list[edge].target];
+                BOOST_ASSERT(edge_list[edge].target != SPECIAL_NODEID);
                 old_to_new_edge[edge] = new_edge_index++;
             }
             node_array[node].first_edge = new_first_edge;
@@ -400,20 +462,6 @@ template <typename EdgeDataT> class DynamicGraph
         edge_list[edge].target = (std::numeric_limits<NodeIterator>::max)();
     }
 
-    struct Node
-    {
-        // index of the first edge
-        EdgeIterator first_edge;
-        // amount of edges
-        unsigned edges;
-    };
-
-    struct Edge
-    {
-        NodeIterator target;
-        EdgeDataT data;
-    };
-
     NodeIterator number_of_nodes;
     std::atomic_uint number_of_edges;
 

include/util/exclude_flag.hpp

@@ -0,0 +1,35 @@
+#ifndef OSRM_UTIL_EXCLUDE_FLAG_HPP
+#define OSRM_UTIL_EXCLUDE_FLAG_HPP
+
+#include "extractor/node_data_container.hpp"
+#include "extractor/profile_properties.hpp"
+
+namespace osrm
+{
+namespace util
+{
+
+inline std::vector<std::vector<bool>>
+excludeFlagsToNodeFilter(const NodeID number_of_nodes,
+                         const extractor::EdgeBasedNodeDataContainer &node_data,
+                         const extractor::ProfileProperties &properties)
+{
+    std::vector<std::vector<bool>> filters;
+    for (auto mask : properties.excludable_classes)
+    {
+        if (mask != extractor::INAVLID_CLASS_DATA)
+        {
+            std::vector<bool> allowed_nodes(number_of_nodes);
+            for (const auto node : util::irange<NodeID>(0, number_of_nodes))
+            {
+                allowed_nodes[node] = (node_data.GetClassData(node) & mask) == 0;
+            }
+            filters.push_back(std::move(allowed_nodes));
+        }
+    }
+    return filters;
+}
+}
+}
+
+#endif

include/util/filtered_graph.hpp

@@ -0,0 +1,165 @@
+#ifndef OSRM_UTIL_FILTERED_GRAPH_HPP
+#define OSRM_UTIL_FILTERED_GRAPH_HPP
+
+#include "storage/shared_memory_ownership.hpp"
+
+#include "util/dynamic_graph.hpp"
+#include "util/filtered_integer_range.hpp"
+#include "util/static_graph.hpp"
+#include "util/vector_view.hpp"
+
+namespace osrm
+{
+namespace util
+{
+namespace detail
+{
+template <typename GraphT, storage::Ownership Ownership> class FilteredGraphImpl;
+
+// For static graphs we can save the filters as a static vector since
+// we don't modify the structure of the graph. This also makes it easy to
+// swap out the filter.
+template <typename EdgeDataT, storage::Ownership Ownership>
+class FilteredGraphImpl<util::StaticGraph<EdgeDataT, Ownership>, Ownership>
+{
+    template <typename T> using Vector = util::ViewOrVector<T, Ownership>;
+
+  public:
+    using Graph = util::StaticGraph<EdgeDataT, Ownership>;
+    using EdgeIterator = typename Graph::EdgeIterator;
+    using NodeIterator = typename Graph::NodeIterator;
+    using NodeArrayEntry = typename Graph::NodeArrayEntry;
+    using EdgeArrayEntry = typename Graph::EdgeArrayEntry;
+    using EdgeRange = util::filtered_range<EdgeIterator, Vector<bool>>;
+
+    unsigned GetNumberOfNodes() const { return graph.GetNumberOfNodes(); }
+
+    unsigned GetNumberOfEdges() const { return graph.GetNumberOfEdges(); }
+
+    unsigned GetOutDegree(const NodeIterator n) const
+    {
+        auto range = graph.GetAdjacentEdgeRange(n);
+        return std::count_if(range.begin(), range.end(), [this](const EdgeIterator edge) {
+            return edge_filter[edge];
+        });
+    }
+
+    inline NodeIterator GetTarget(const EdgeIterator e) const
+    {
+        BOOST_ASSERT(edge_filter[e]);
+        return graph.GetTarget(e);
+    }
+
+    auto &GetEdgeData(const EdgeIterator e)
+    {
+        BOOST_ASSERT(edge_filter[e]);
+        return graph.GetEdgeData(e);
+    }
+
+    const auto &GetEdgeData(const EdgeIterator e) const
+    {
+        BOOST_ASSERT(edge_filter[e]);
+        return graph.GetEdgeData(e);
+    }
+
+    auto GetAdjacentEdgeRange(const NodeIterator n) const
+    {
+        return EdgeRange{graph.BeginEdges(n), graph.EndEdges(n), edge_filter};
+    }
+
+    // searches for a specific edge
+    EdgeIterator FindEdge(const NodeIterator from, const NodeIterator to) const
+    {
+        for (const auto edge : GetAdjacentEdgeRange(from))
+        {
+            if (to == GetTarget(edge))
+            {
+                return edge;
+            }
+        }
+        return SPECIAL_EDGEID;
+    }
+
+    template <typename FilterFunction>
+    EdgeIterator
+    FindSmallestEdge(const NodeIterator from, const NodeIterator to, FilterFunction &&filter) const
+    {
+        static_assert(traits::HasDataMember<typename Graph::EdgeArrayEntry>::value,
+                      "Filtering on .data not possible without .data member attribute");
+
+        EdgeIterator smallest_edge = SPECIAL_EDGEID;
+        EdgeWeight smallest_weight = INVALID_EDGE_WEIGHT;
+        for (auto edge : GetAdjacentEdgeRange(from))
+        {
+            const NodeID target = GetTarget(edge);
+            const auto &data = GetEdgeData(edge);
+            if (target == to && data.weight < smallest_weight &&
+                std::forward<FilterFunction>(filter)(data))
+            {
+                smallest_edge = edge;
+                smallest_weight = data.weight;
+            }
+        }
+        return smallest_edge;
+    }
+
+    EdgeIterator FindEdgeInEitherDirection(const NodeIterator from, const NodeIterator to) const
+    {
+        EdgeIterator tmp = FindEdge(from, to);
+        return (SPECIAL_NODEID != tmp ? tmp : FindEdge(to, from));
+    }
+
+    EdgeIterator
+    FindEdgeIndicateIfReverse(const NodeIterator from, const NodeIterator to, bool &result) const
+    {
+        EdgeIterator current_iterator = FindEdge(from, to);
+        if (SPECIAL_NODEID == current_iterator)
+        {
+            current_iterator = FindEdge(to, from);
+            if (SPECIAL_NODEID != current_iterator)
+            {
+                result = true;
+            }
+        }
+        return current_iterator;
+    }
+
+    FilteredGraphImpl() = default;
+
+    FilteredGraphImpl(Graph graph, Vector<bool> edge_filter_)
+        : graph(std::move(graph)), edge_filter(std::move(edge_filter_))
+    {
+        BOOST_ASSERT(edge_filter.empty() || edge_filter.size() == graph.GetNumberOfEdges());
+    }
+
+    // Takes a graph and a function that maps EdgeID to true
+    // if the edge should be included in the graph.
+    template <typename Pred>
+    FilteredGraphImpl(Graph graph, Pred filter)
+        : graph(std::move(graph)), edge_filter(graph.GetNumberOfEdges())
+    {
+        auto edge_ids = util::irange<EdgeID>(0, graph.GetNumberOfEdges());
+        std::transform(edge_ids.begin(), edge_ids.end(), edge_filter.begin(), filter);
+    }
+
+    void Renumber(const std::vector<NodeID> &old_to_new_node)
+    {
+        graph.Renumber(old_to_new_node);
+        // FIXME the edge filter needs to be renumbered with a different permutation
+        // util::inplacePermutation(edge_filter.begin(), edge_filter.end(), old_to_new_node);
+    }
+
+  private:
+    Graph graph;
+    Vector<bool> edge_filter;
+};
+}
+
+template <typename GraphT>
+using FilteredGraphContainer = detail::FilteredGraphImpl<GraphT, storage::Ownership::Container>;
+template <typename GraphT>
+using FilteredGraphView = detail::FilteredGraphImpl<GraphT, storage::Ownership::View>;
+}
+}
+
+#endif

include/util/filtered_integer_range.hpp

@@ -0,0 +1,101 @@
+#ifndef FILTERED_INTEGER_RANGE_HPP
+#define FILTERED_INTEGER_RANGE_HPP
+
+#include <boost/assert.hpp>
+#include <boost/iterator/iterator_facade.hpp>
+
+#include <type_traits>
+
+#include <cstdint>
+
+namespace osrm
+{
+namespace util
+{
+
+// This implements a single-pass integer range.
+// We need our own implementation here because using boost::adaptor::filtered() has
+// the problem that the return-type depends on the lambda-type you pass into the function.
+// That makes it unsuitable to use in interface where we would expect all filtered ranges
+// to be off the same type.
+
+template <typename Integer, typename Filter>
+class filtered_integer_iterator
+    : public boost::iterator_facade<filtered_integer_iterator<Integer, Filter>,
+                                    Integer,
+                                    boost::single_pass_traversal_tag,
+                                    Integer>
+{
+    typedef boost::iterator_facade<filtered_integer_iterator<Integer, Filter>,
+                                   Integer,
+                                   boost::single_pass_traversal_tag,
+                                   Integer>
+        base_t;
+
+  public:
+    typedef typename base_t::value_type value_type;
+    typedef typename base_t::difference_type difference_type;
+    typedef typename base_t::reference reference;
+    typedef std::random_access_iterator_tag iterator_category;
+
+    filtered_integer_iterator() : value(), filter(nullptr) {}
+    explicit filtered_integer_iterator(value_type x, value_type end_value, const Filter *filter)
+        : value(x), end_value(end_value), filter(filter)
+    {
+    }
+
+  private:
+    void increment()
+    {
+        do
+        {
+            ++value;
+        } while (value < end_value && !(*filter)[value]);
+    }
+    bool equal(const filtered_integer_iterator &other) const { return value == other.value; }
+    reference dereference() const { return value; }
+
+    friend class ::boost::iterator_core_access;
+    value_type value;
+    value_type end_value;
+    const Filter *filter;
+};
+
+template <typename Integer, typename Filter> class filtered_range
+{
+  public:
+    typedef filtered_integer_iterator<Integer, Filter> const_iterator;
+    typedef filtered_integer_iterator<Integer, Filter> iterator;
+
+    filtered_range(Integer begin, Integer end, const Filter &filter) : last(end, end, &filter)
+    {
+        while (begin < end && !filter[begin])
+        {
+            begin++;
+        }
+
+        iter = iterator(begin, end, &filter);
+    }
+
+    iterator begin() const noexcept { return iter; }
+    iterator end() const noexcept { return last; }
+
+  private:
+    iterator iter;
+    iterator last;
+};
+
+// convenience function to construct an integer range with type deduction
+template <typename Integer, typename Filter>
+filtered_range<Integer, Filter>
+filtered_irange(const Integer first,
+                const Integer last,
+                const Filter &filter,
+                typename std::enable_if<std::is_integral<Integer>::value>::type * = 0) noexcept
+{
+    return filtered_range<Integer, Filter>(first, last, filter);
+}
+}
+}
+
+#endif // INTEGER_RANGE_HPP

include/util/query_heap.hpp

@@ -152,6 +152,7 @@ class QueryHeap
 
     void Insert(NodeID node, Weight weight, const Data &data)
     {
+        BOOST_ASSERT(node < std::numeric_limits<NodeID>::max());
         const auto index = static_cast<Key>(inserted_nodes.size());
         const auto handle = heap.push(std::make_pair(weight, index));
         inserted_nodes.emplace_back(HeapNode{handle, node, weight, data});