[traffic] Truncate locations which overshoot their endpoints

Signed-off-by: mvglasow <michael -at- vonglasow.com>

data/test_data/traff/LT-A5-Kaunas-Eastbound.xml

@@ -0,0 +1,23 @@
+<!--
+    This is a 140 m location. Reference points are almost exactly on the opposite
+    carriageway; since this is inside a junction, there is a wider gap between
+    the two carriageways than there would be on a normal stretch of expressway.
+    Without truncation, the decoded location starts approximately in the right
+    spot but overshoots the end point, going to the nearest junction, then back
+    in the opposite direction and to the end point on the opposite carriageway,
+    ending within 5 m of the end point.
+-->
+<feed>
+   <message id="lt.eismoinfo.restrictions:4249b6510b73750684ca94de5fe8cf32,eastbound" receive_time="2025-01-21T12:33:06Z" update_time="2025-01-21T12:33:06Z" expiration_time="2025-12-31T21:00:00Z" cancellation="false" forecast="false" urgency="NORMAL">
+      <merge/>
+      <location country="LT" origin="Suvalkai*" directionality="ONE_DIRECTION" destination="Kaunas" road_class="TRUNK" road_ref="A5">
+         <from distance="0.14">54.939945 23.879789</from>
+         <to distance="0.0" junction_name="Kaunas">54.940094 23.881950</to>
+      </location>
+      <events>
+         <event class="RESTRICTION" type="RESTRICTION_MAX_WIDTH" q_dimension="3.5"/>
+         <event class="CONSTRUCTION" type="CONSTRUCTION_ROADWORKS"/>
+         <event class="RESTRICTION" type="RESTRICTION_SPEED_LIMIT" speed="50"/>
+      </events>
+   </message>
+</feed>

libs/traffxml/traff_decoder.cpp

@@ -815,6 +815,63 @@ void RoutingTraffDecoder::AddDecodedSegment(traffxml::MultiMwmColoring & decoded
   decoded[mwmId][traffic::TrafficInfo::RoadSegmentId(fid, sid, direction)] = traffic::SpeedGroup::Unknown;
 }
 
+void RoutingTraffDecoder::TruncateRoute(std::vector<routing::RouteSegment> & rsegments,
+                                        routing::Checkpoints const & checkpoints)
+{
+  double const endWeight = rsegments.back().GetTimeFromBeginningSec();
+
+  // erase leading and trailing fake segments
+  while(!rsegments.empty() && rsegments.front().GetSegment().GetMwmId() == routing::kFakeNumMwmId)
+    rsegments.erase(rsegments.begin());
+  while(!rsegments.empty() && rsegments.back().GetSegment().GetMwmId() == routing::kFakeNumMwmId)
+    rsegments.pop_back();
+
+  if (rsegments.size() < 2)
+    return;
+
+  // Index of first segment to keep, or number of segments to truncate at start.
+  size_t start = 0;
+  // Cost saved by omitting all segments prior to `start`.
+  double startSaving = 0;
+
+  // Index of last segment to keep.
+  size_t end = rsegments.size() - 1;
+  // Cost saved by omitting the last `end` segments.
+  double endSaving = 0;
+
+  TruncateStart(rsegments, checkpoints, start, startSaving);
+  TruncateEnd(rsegments, checkpoints, end, endSaving, endWeight);
+
+  /*
+   * If start <= end, we can truncate both ends at the same time.
+   * Else, the segments to truncate overlap. In this case, first truncate where the saving is bigger,
+   * then recalculate the other end and truncate it as well.
+   */
+  if (start <= end)
+  {
+    rsegments.erase(rsegments.begin() + end + 1, rsegments.end());
+    rsegments.erase(rsegments.begin(), rsegments.begin() + start);
+  }
+  else if (startSaving > endSaving)
+  {
+    // truncate start, then recalculate and truncate end
+    rsegments.erase(rsegments.begin(), rsegments.begin() + start);
+    end = rsegments.size() - 1;
+    endSaving = 0;
+    TruncateEnd(rsegments, checkpoints, end, endSaving, endWeight);
+    rsegments.erase(rsegments.begin() + end + 1, rsegments.end());
+  }
+  else
+  {
+    // truncate end, then recalculate and truncate start
+    rsegments.erase(rsegments.begin() + end + 1, rsegments.end());
+    start = 0;
+    startSaving = 0;
+    TruncateStart(rsegments, checkpoints, start, startSaving);
+    rsegments.erase(rsegments.begin(), rsegments.begin() + start);
+  }
+}
+
 void RoutingTraffDecoder::DecodeLocationDirection(traffxml::TraffMessage & message,
                                                   traffxml::MultiMwmColoring & decoded, bool backwards)
 {
@@ -909,11 +966,7 @@ void RoutingTraffDecoder::DecodeLocationDirection(traffxml::TraffMessage & messa
   {
     std::vector<routing::RouteSegment> rsegments(route->GetRouteSegments());
 
-    // erase leading and trailing fake segments
-    while(!rsegments.empty() && rsegments.front().GetSegment().GetMwmId() == routing::kFakeNumMwmId)
-      rsegments.erase(rsegments.begin());
-    while(!rsegments.empty() && rsegments.back().GetSegment().GetMwmId() == routing::kFakeNumMwmId)
-      rsegments.pop_back();
+    TruncateRoute(rsegments, checkpoints);
 
     if (!backwards && message.m_location.value().m_at && !message.m_location.value().m_to)
       // from–at in forward direction, add last segment
@@ -1134,4 +1187,38 @@ std::vector<std::string> ParseRef(std::string & ref)
     res.push_back(curr);
   return res;
 }
+
+void TruncateStart(std::vector<routing::RouteSegment> & rsegments,
+                   routing::Checkpoints const & checkpoints,
+                   size_t & start, double & startSaving)
+{
+  for (size_t i = 0; i < rsegments.size(); i++)
+  {
+    double newStartSaving = rsegments[i].GetTimeFromBeginningSec()
+        - (mercator::DistanceOnEarth(checkpoints.GetStart(), rsegments[i].GetJunction().GetPoint())
+            * kOffroadPenalty);
+    if (newStartSaving > startSaving)
+    {
+      start = i + 1; // add 1 because we are ditching this segment and keeping the next one
+      startSaving = newStartSaving;
+    }
+  }
+}
+
+void TruncateEnd(std::vector<routing::RouteSegment> & rsegments,
+                 routing::Checkpoints const & checkpoints,
+                 size_t & end, double & endSaving, double const endWeight)
+{
+  for (size_t i = 0; i < rsegments.size(); i++)
+  {
+    double newEndSaving = endWeight - rsegments[i].GetTimeFromBeginningSec()
+        - (mercator::DistanceOnEarth(rsegments[i].GetJunction().GetPoint(), checkpoints.GetFinish())
+            * kOffroadPenalty);
+    if (newEndSaving > endSaving)
+    {
+      end = i;
+      endSaving = newEndSaving;
+    }
+  }
+}
 }  // namespace traffxml

libs/traffxml/traff_decoder.hpp

@@ -382,6 +382,30 @@ protected:
    */
   void DecodeLocation(traffxml::TraffMessage & message, traffxml::MultiMwmColoring & decoded) override;
 
+  /**
+   * @brief Truncates the route so its endpoints best match the reference points.
+   *
+   * Leading and trailing fake segments are discarded.
+   *
+   * When building the graph, the router creates fake segments to the nearest roads. These are not
+   * necessarily the best for location decoding, which may result in “heads” or “tails” being added
+   * to the decoded location. This function attempts to detect and remove them.
+   *
+   * To do this, it iterates over the nodes (taken from `rsegments`) and determines if any of them
+   * is a better start/end candidate. This is done by calculating the cost of leaping between the
+   * node and the corresponding checkpoint; if this is cheaper than the stretch of route bypassed
+   * in this way, the node becomes a candidate for the corresponding endpoint. The higher the cost
+   * saving, the better the candidate.
+   *
+   * After identifying the best candidate for each endpoint, segments outside these nodes are
+   * discarded.
+   *
+   * @param rsegments The segments of the route
+   * @param checkpoints The reference points (at least two)
+   */
+  void TruncateRoute(std::vector<routing::RouteSegment> & rsegments,
+                     routing::Checkpoints const & checkpoints);
+
 private:
   static void LogCode(routing::RouterResultCode code, double const elapsedSec);
 
@@ -433,4 +457,39 @@ bool IsRamp(routing::HighwayType highwayType);
  * would be broken down into `a, 4, 2`.
  */
 std::vector<std::string> ParseRef(std::string & ref);
+
+/**
+ * @brief Calculates the segments to truncate at the start of the route.
+ *
+ * The route is not actually truncated by this function.
+ *
+ * `start` and `startSaving` should be 0 when calling this function. After it returns, these values
+ * will indicate the first segment to keep and the cost saved by truncating everything before.
+ *
+ * @param rsegments The segments of the route
+ * @param checkpoints The reference points (at least two)
+ * @param start Index of the first segment to keep
+ * @param startSaving Cost saved by truncating
+ */
+void TruncateStart(std::vector<routing::RouteSegment> & rsegments,
+                   routing::Checkpoints const & checkpoints,
+                   size_t & start, double & startSaving);
+
+/**
+ * @brief Calculates the segments to truncate at the start of the route.
+ *
+ * The route is not actually truncated by this function.
+ *
+ * `end` should be `rsegments.size() - 1` and `endSaving` should be 0 when calling this function.
+ * After it returns, these values will indicate the last segment to keep and the cost saved by
+ * truncating everything after.
+ *
+ * @param rsegments The segments of the route
+ * @param checkpoints The reference points (at least two)
+ * @param end Index of the last segment to keep
+ * @param endSaving Cost saved by truncating
+ */
+void TruncateEnd(std::vector<routing::RouteSegment> & rsegments,
+                 routing::Checkpoints const & checkpoints,
+                 size_t & end, double & endSaving, double const endWeight);
 }  // namespace traffxml