Insert Shape Points at TrafficSpeed Breakpoints

CHANGELOG.md

@@ -109,6 +109,7 @@
    * ADDED: Support for 64bit osm node ids in parsing stage of tile building [#2422](https://github.com/valhalla/valhalla/pull/2422)
    * CHANGED: Point2/PointLL are now templated to allow for higher precision coordinate math when desired [#2429](https://github.com/valhalla/valhalla/pull/2429)
    * ADDED: Optional OpenLR Encoded Path Edges in API Response [#2424](https://github.com/valhalla/valhalla/pull/2424)
+   * ADDED: Properly split returned path if traffic conditions change partway along edges [#2451](https://github.com/valhalla/valhalla/pull/2451/files)
 
 ## Release Date: 2019-11-21 Valhalla 3.0.9
 * **Bug Fix**

src/midgard/pointll.cc

@@ -1,17 +1,17 @@
-#include <valhalla/midgard/pointll.h>
+#include "midgard/pointll.h"
 
 #include <list>
 
 namespace valhalla {
 namespace midgard {
 
-/**
- * Gets the midpoint on a line segment between this point and point p1.
- * @param   p1  Point
- * @return  Returns the midpoint between this point and p1.
- */
 template <typename PrecisionT>
-GeoPoint<PrecisionT> GeoPoint<PrecisionT>::MidPoint(const GeoPoint<PrecisionT>& p) const {
+GeoPoint<PrecisionT> GeoPoint<PrecisionT>::PointAlongSegment(const GeoPoint<PrecisionT>& p,
+                                                             PrecisionT distance) const {
+  if (distance == 0)
+    return *this;
+  if (distance == 1)
+    return p;
   // radians
   const auto lon1 = first * -RAD_PER_DEG;
   const auto lat1 = second * RAD_PER_DEG;
@@ -25,13 +25,15 @@ GeoPoint<PrecisionT> GeoPoint<PrecisionT>::MidPoint(const GeoPoint<PrecisionT>&
   // fairly accurate distance between points
   const auto d = acos(sl1 * sl2 + cl1 * cl2 * cos(lon1 - lon2));
   // interpolation parameters
-  const auto ab = sin(d * .5) / sin(d);
-  const auto acs1 = ab * cl1;
-  const auto bcs2 = ab * cl2;
+  auto sd = sin(d);
+  const auto a = sin(d * (1 - distance)) / sd;
+  const auto b = sin(d * distance) / sd;
+  const auto acs1 = a * cl1;
+  const auto bcs2 = b * cl2;
   // find the interpolated point along the arc
   const auto x = acs1 * cos(lon1) + bcs2 * cos(lon2);
   const auto y = acs1 * sin(lon1) + bcs2 * sin(lon2);
-  const auto z = ab * (sl1 + sl2);
+  const auto z = a * sl1 + b * sl2;
   return GeoPoint<PrecisionT>(atan2(y, x) * -DEG_PER_RAD,
                               atan2(z, sqrt(x * x + y * y)) * DEG_PER_RAD);
 }

src/midgard/util.cc

@@ -78,7 +78,7 @@ template <class container_t> container_t trim_front(container_t& pts, const floa
     double segdist = p1->Distance(*p2);
     if ((d + segdist) > dist) {
       double frac = (dist - d) / segdist;
-      auto midpoint = p1->AffineCombination((1.0 - frac), frac, *p2);
+      auto midpoint = p1->PointAlongSegment(*p2, frac);
       result.push_back(midpoint);
 
       // Remove used part of polyline
@@ -152,7 +152,7 @@ float tangent_angle(size_t index,
     // are we done yet?
     if (remaining <= d) {
       auto coef = remaining / d;
-      u = u.AffineCombination(1 - coef, coef, *i);
+      u = u.PointAlongSegment(*i, coef);
       return u.Heading(point);
     }
     // next one
@@ -170,7 +170,7 @@ float tangent_angle(size_t index,
     // are we done yet?
     if (remaining <= d) {
       auto coef = remaining / d;
-      v = v.AffineCombination(1 - coef, coef, *i);
+      v = v.PointAlongSegment(*i, coef);
       return u.Heading(v);
     }
     // next one

src/mjolnir/valhalla_fetch_transit.cc

@@ -203,8 +203,8 @@ std::priority_queue<weighted_tile_t> which_tiles(const ptree& pt, const std::str
       }
 
       // expand the top and bottom edges of the box to account for geodesics
-      min_y -= std::abs(min_y - PointLL(min_x, min_y).MidPoint({max_x, min_y}).second);
-      max_y += std::abs(max_y - PointLL(min_x, max_y).MidPoint({max_x, max_y}).second);
+      min_y -= std::abs(min_y - PointLL(min_x, min_y).PointAlongSegment({max_x, min_y}).second);
+      max_y += std::abs(max_y - PointLL(min_x, max_y).PointAlongSegment({max_x, max_y}).second);
       auto min_c = tile_level.tiles.Col(min_x), min_r = tile_level.tiles.Row(min_y);
       auto max_c = tile_level.tiles.Col(max_x), max_r = tile_level.tiles.Row(max_y);
       if (min_c > max_c) {
@@ -230,9 +230,10 @@ std::priority_queue<weighted_tile_t> which_tiles(const ptree& pt, const std::str
   ++utc->tm_mon;
   for (const auto& tile : tiles) {
     auto bbox = tile_level.tiles.TileBounds(tile.tileid());
-    auto min_y = std::max(bbox.miny(), bbox.minpt().MidPoint({bbox.maxx(), bbox.miny()}).second);
-    auto max_y =
-        std::min(bbox.maxy(), PointLL(bbox.minx(), bbox.maxy()).MidPoint(bbox.maxpt()).second);
+    auto min_y =
+        std::max(bbox.miny(), bbox.minpt().PointAlongSegment({bbox.maxx(), bbox.miny()}).second);
+    auto max_y = std::min(bbox.maxy(),
+                          PointLL(bbox.minx(), bbox.maxy()).PointAlongSegment(bbox.maxpt()).second);
     bbox = AABB2<PointLL>(bbox.minx(), min_y, bbox.maxx(), max_y);
     // stop count
     auto request =
@@ -747,12 +748,12 @@ void fetch_tiles(const ptree& pt,
     uniques.lock.unlock();
     auto filter = tiles.TileBounds(current.tileid());
     // expanding both top and bottom by distance to geodesic running through the coords
-    auto min_y =
-        filter.miny() -
-        std::abs(filter.miny() - filter.minpt().MidPoint({filter.maxx(), filter.miny()}).second);
-    auto max_y =
-        filter.maxy() +
-        std::abs(filter.maxy() - filter.maxpt().MidPoint({filter.minx(), filter.maxy()}).second);
+    auto min_y = filter.miny() -
+                 std::abs(filter.miny() -
+                          filter.minpt().PointAlongSegment({filter.maxx(), filter.miny()}).second);
+    auto max_y = filter.maxy() +
+                 std::abs(filter.maxy() -
+                          filter.maxpt().PointAlongSegment({filter.minx(), filter.maxy()}).second);
     AABB2<PointLL> bbox(filter.minx(), min_y, filter.maxx(), max_y);
     ptree response;
     auto api_key =

src/sif/transitcost.cc

@@ -538,10 +538,6 @@ bool TransitCost::Allowed(const baldr::DirectedEdge* edge,
                           const uint64_t current_time,
                           const uint32_t tz_index,
                           int& restriction_idx) const {
-  if (flow_mask_ & kCurrentFlowMask) {
-    if (tile->IsClosedDueToTraffic(edgeid))
-      return false;
-  }
   // TODO - obtain and check the access restrictions.
 
   if (exclude_stops_.size()) {
@@ -556,9 +552,9 @@ bool TransitCost::Allowed(const baldr::DirectedEdge* edge,
   }
 
   if (edge->use() == Use::kBus) {
-    return (use_bus_ > 0.0f) ? true : false;
+    return use_bus_ > 0.0f;
   } else if (edge->use() == Use::kRail) {
-    return (use_rail_ > 0.0f) ? true : false;
+    return use_rail_ > 0.0f;
   }
   return true;
 }

src/thor/trace_route_action.cc

@@ -313,10 +313,8 @@ void thor_worker_t::build_trace(
 
   // initialize the origin and destination location for route
   const meili::EdgeSegment* origin_segment = paths.front().second.front();
-  std::cout << *origin_segment << std::endl;
   const meili::MatchResult& origin_match = match_results[origin_segment->first_match_idx];
   const meili::EdgeSegment* dest_segment = paths.back().second.back();
-  std::cout << *dest_segment << std::endl;
   const meili::MatchResult& dest_match = match_results[dest_segment->last_match_idx];
   Location* origin_location = options.mutable_shape(&origin_match - &match_results.front());
   Location* destination_location = options.mutable_shape(&dest_match - &match_results.front());

src/thor/triplegbuilder.cc

@@ -92,25 +92,65 @@ void AssignAdmins(const AttributesController& controller,
 void SetShapeAttributes(const AttributesController& controller,
                         const GraphTile* tile,
                         const DirectedEdge* edge,
-                        const std::shared_ptr<sif::DynamicCost>& costing,
-                        std::vector<PointLL>::const_iterator shape_begin,
-                        std::vector<PointLL>::const_iterator shape_end,
+                        std::vector<PointLL>& shape,
+                        size_t shape_begin,
                         TripLeg& trip_path,
-                        uint32_t second_of_week,
-                        double edge_percentage) {
+                        double src_pct,
+                        double tgt_pct,
+                        double edge_seconds,
+                        bool cut_for_traffic) {
+  // TODO: if this is a transit edge then the costing will throw
+
   if (trip_path.has_shape_attributes()) {
-    // calculates total edge time and total edge length
-    // TODO: you can get this directly from the path edge by taking its cost and subtracting off
-    // the transition cost that it also now contains
-    double edge_time =
-        costing->EdgeCost(edge, tile, second_of_week).secs * edge_percentage; // seconds
-    // TODO: get the measured length from shape (full shape) to increase precision
-    double edge_length = edge->length() * edge_percentage; // meters
+    // A list of percent along the edge and corresponding speed (meters per second)
+    std::vector<std::tuple<double, double>> speeds;
+    double speed = (edge->length() * (tgt_pct - src_pct)) / edge_seconds;
+    if (cut_for_traffic) {
+      // TODO: we'd like to use the speed from traffic here but because there are synchronization
+      // problems with those records changing between when we used them to make the path and when we
+      // try to grab them again here, we instead rely on the total time from PathInfo and just do the
+      // cutting for now
+      const auto& traffic_speed = tile->trafficspeed(edge);
+      if (traffic_speed.breakpoint1 > 0) {
+        speeds.emplace_back(traffic_speed.breakpoint1 / 255.0, speed);
+        if (traffic_speed.breakpoint2 > 0) {
+          speeds.emplace_back(traffic_speed.breakpoint2 / 255.0, speed);
+          if (traffic_speed.speed3 != UNKNOWN_TRAFFIC_SPEED_RAW) {
+            speeds.emplace_back(1, speed);
+          }
+        }
+      }
+    }
+    // Cap the end so that we always have something to use
+    if (speeds.empty() || std::get<0>(speeds.back()) < tgt_pct) {
+      speeds.emplace_back(tgt_pct, speed);
+    }
+
     // Set the shape attributes
-    for (++shape_begin; shape_begin < shape_end; ++shape_begin) {
-      double distance = shape_begin->Distance(*(shape_begin - 1)); // meters
-      double distance_pct = distance / edge_length;                // fraction of edge length
-      double time = edge_time * distance_pct;                      // seconds
+    double distance_total_pct = src_pct;
+    auto speed_itr = std::find_if(speeds.cbegin(), speeds.cend(),
+                                  [distance_total_pct](const decltype(speeds)::value_type& s) {
+                                    return distance_total_pct <= std::get<0>(s);
+                                  });
+    for (auto i = shape_begin + 1; i < shape.size(); ++i) {
+      // If there is a change in speed here we need to make a new shape point and continue from
+      // there
+      double distance = shape[i].Distance(shape[i - 1]); // meters
+      double distance_pct = distance / edge->length();
+      double next_total = distance_total_pct + distance_pct;
+      size_t shift = 0;
+      if (next_total > std::get<0>(*speed_itr) && std::next(speed_itr) != speeds.cend()) {
+        // Calculate where the cut point should be between these two existing shape points
+        auto coef =
+            (std::get<0>(*speed_itr) - distance_total_pct) / (next_total - distance_total_pct);
+        auto point = shape[i - 1].PointAlongSegment(shape[i], coef);
+        shape.insert(shape.begin() + i, point);
+        next_total = std::get<0>(*speed_itr);
+        distance *= coef;
+        shift = 1;
+      }
+      distance_total_pct = next_total;
+      double time = distance / std::get<1>(*speed_itr); // seconds
 
       // Set shape attributes time per shape point if requested
       if (controller.attributes.at(kShapeAttributesTime)) {
@@ -129,6 +169,9 @@ void SetShapeAttributes(const AttributesController& controller,
         // convert speed to decimeters per sec and then round to an integer
         trip_path.mutable_shape_attributes()->add_speed((distance * kDecimeterPerMeter / time) + 0.5);
       }
+
+      // If we just cut the shape we need to go on to the next marker only after setting the attribs
+      std::advance(speed_itr, shift);
     }
   }
 }
@@ -325,7 +368,7 @@ TripLeg_Use GetTripLegUse(const Use use) {
       return TripLeg_Use_kPlatformConnectionUse;
     case Use::kTransitConnection:
       return TripLeg_Use_kTransitConnectionUse;
-    // Should not see other values
+      // Should not see other values
     default:
       // TODO should we throw a runtime error?
       return TripLeg_Use_kRoadUse;
@@ -689,10 +732,11 @@ TripLeg_Edge* AddTripEdge(const AttributesController& controller,
 
   // Set speed if requested
   if (controller.attributes.at(kEdgeSpeed)) {
+    // TODO: if this is a transit edge then the costing will throw
     // TODO: could get better precision speed here by calling GraphTile::GetSpeed but we'd need to
-    // know whether or not the costing actually cares about the speed of the edge. Perhaps a refactor
-    // of costing to have a GetSpeed function which EdgeCost calls internally but which we can also
-    // call externally
+    // know whether or not the costing actually cares about the speed of the edge. Perhaps a
+    // refactor of costing to have a GetSpeed function which EdgeCost calls internally but which we
+    // can also call externally
     auto speed = directededge->length() /
                  costing->EdgeCost(directededge, graphtile, second_of_week).secs * 3.6;
     trip_edge->set_speed(speed);
@@ -1249,10 +1293,10 @@ void TripLegBuilder::Build(
     bool drive_on_right = graphreader.nodeinfo(start_node)->drive_on_right();
 
     // Add trip edge
+    auto costing = mode_costing[static_cast<uint32_t>(path_begin->mode)];
     auto trip_edge =
         AddTripEdge(controller, path_begin->edgeid, path_begin->trip_id, 0, path_begin->mode,
-                    travel_types[static_cast<int>(path_begin->mode)],
-                    mode_costing[static_cast<uint32_t>(path_begin->mode)], edge, drive_on_right,
+                    travel_types[static_cast<int>(path_begin->mode)], costing, edge, drive_on_right,
                     trip_path.add_node(), tile, graphreader, origin_second_of_week, startnode.id(),
                     false, nullptr, path_begin->restriction_index);
 
@@ -1262,6 +1306,11 @@ void TripLegBuilder::Build(
       trip_edge->set_length(km);
     }
 
+    // Set shape attributes
+    auto edge_seconds = path_begin->elapsed_time - path_begin->turn_cost;
+    SetShapeAttributes(controller, tile, edge, shape, 0, trip_path, start_pct, end_pct, edge_seconds,
+                       costing->flow_mask() & kCurrentFlowMask);
+
     // Set begin shape index if requested
     if (controller.attributes.at(kEdgeBeginShapeIndex)) {
       trip_edge->set_begin_shape_index(0);
@@ -1271,11 +1320,6 @@ void TripLegBuilder::Build(
       trip_edge->set_end_shape_index(shape.size() - 1);
     }
 
-    // Set shape attributes
-    SetShapeAttributes(controller, tile, edge, mode_costing[static_cast<int>(path_begin->mode)],
-                       shape.begin(), shape.end(), trip_path, origin_second_of_week,
-                       end_pct - start_pct);
-
     // Set begin and end heading if requested. Uses shape so
     // must be done after the edge's shape has been added.
     SetHeadings(trip_edge, controller, edge, shape, 0);
@@ -1595,7 +1639,8 @@ void TripLegBuilder::Build(
 
     // some information regarding shape/length trimming
     auto is_last_edge = edge_itr == (path_end - 1);
-    float length_pct = (is_first_edge ? 1.f - start_pct : (is_last_edge ? end_pct : 1.f));
+    float trim_start_pct = is_first_edge ? start_pct : 0;
+    float trim_end_pct = is_last_edge ? end_pct : 1;
 
     // Process the shape for edges where a route discontinuity occurs
     if (edge_trimming && !edge_trimming->empty() && edge_trimming->count(edge_index) > 0) {
@@ -1624,7 +1669,8 @@ void TripLegBuilder::Build(
       }
 
       // Overwrite the trimming information for the edge length now that we know what it is
-      length_pct = edge_end_info.distance_along - edge_begin_info.distance_along;
+      trim_start_pct = edge_begin_info.distance_along;
+      trim_end_pct = edge_end_info.distance_along;
 
       // Trim the shape
       auto edge_length = static_cast<float>(directededge->length());
@@ -1671,10 +1717,19 @@ void TripLegBuilder::Build(
 
     // Set length if requested. Convert to km
     if (controller.attributes.at(kEdgeLength)) {
-      float km = std::max(directededge->length() * kKmPerMeter * length_pct, 0.001f);
+      float km =
+          std::max(directededge->length() * kKmPerMeter * (trim_end_pct - trim_start_pct), 0.001f);
       trip_edge->set_length(km);
     }
 
+    // Set shape attributes
+    auto edge_seconds = edge_itr->elapsed_time - edge_itr->turn_cost;
+    if (edge_itr != path_begin)
+      edge_seconds -= std::prev(edge_itr)->elapsed_time;
+    SetShapeAttributes(controller, graphtile, directededge, trip_shape, begin_index, trip_path,
+                       trim_start_pct, trim_end_pct, edge_seconds,
+                       costing->flow_mask() & kCurrentFlowMask);
+
     // Set begin shape index if requested
     if (controller.attributes.at(kEdgeBeginShapeIndex)) {
       trip_edge->set_begin_shape_index(begin_index);
@@ -1685,10 +1740,6 @@ void TripLegBuilder::Build(
       trip_edge->set_end_shape_index(trip_shape.size() - 1);
     }
 
-    // Set shape attributes
-    SetShapeAttributes(controller, graphtile, directededge, costing, trip_shape.begin() + begin_index,
-                       trip_shape.end(), trip_path, second_of_week, length_pct);
-
     // Set begin and end heading if requested. Uses trip_shape so
     // must be done after the edge's shape has been added.
     SetHeadings(trip_edge, controller, directededge, trip_shape, begin_index);