redoing distance calculation #14760

src/libsumo/Vehicle.cpp

@@ -683,7 +683,7 @@ Vehicle::getDrivingDistance(const std::string& vehID, const std::string& edgeID,
     if (veh->isOnRoad()) {
         const MSEdge* edge = microVeh != nullptr ? &veh->getLane()->getEdge() : veh->getEdge();
         double distance = veh->getRoute().getDistanceBetween(veh->getPositionOnLane(), pos,
-                          edge, Helper::getEdge(edgeID), true, veh->getRoutePosition());
+                          edge, Helper::getEdge(edgeID), veh->getRoutePosition());
         if (distance == std::numeric_limits<double>::max()) {
             return INVALID_DOUBLE_VALUE;
         }
@@ -703,7 +703,7 @@ Vehicle::getDrivingDistance2D(const std::string& vehID, double x, double y) {
     if (veh->isOnRoad()) {
         std::pair<MSLane*, double> roadPos = Helper::convertCartesianToRoadMap(Position(x, y), veh->getVehicleType().getVehicleClass());
         double distance = veh->getRoute().getDistanceBetween(veh->getPositionOnLane(), roadPos.second,
-                          veh->getEdge(), &roadPos.first->getEdge(), true, veh->getRoutePosition());
+                          veh->getEdge(), &roadPos.first->getEdge(), veh->getRoutePosition());
         if (distance == std::numeric_limits<double>::max()) {
             return INVALID_DOUBLE_VALUE;
         }

src/microsim/MSRoute.cpp

@@ -309,57 +309,43 @@ MSRoute::dict_clearState() {
 
 double
 MSRoute::getDistanceBetween(double fromPos, double toPos,
-                            const MSEdge* fromEdge, const MSEdge* toEdge, bool includeInternal, int routePosition) const {
-    //std::cout << SIMTIME << " getDistanceBetween from=" << fromEdge->getID() << " to=" << toEdge->getID() << " fromPos=" << fromPos << " toPos=" << toPos << " includeInternal=" << includeInternal << "\n";
-    if (routePosition < 0 || routePosition >= (int)myEdges.size()) {
-        throw ProcessError("Invalid routePosition " + toString(routePosition) + " for route with " + toString(myEdges.size()) + " edges");
-    }
-    if (fromEdge->isInternal() && toEdge->isInternal() && fromEdge->getToJunction() == toEdge->getToJunction()) {
-        // internal edges within the same junction
-        if (fromEdge == toEdge) {
-            if (fromPos <= toPos) {
-                return toPos - fromPos;
-            }
-        } else if (fromEdge->getSuccessors().front() == toEdge) {
-            return fromEdge->getLength() - fromPos + toPos;
-        }
+                            const MSEdge* fromEdge, const MSEdge* toEdge, int routePosition) const {
+    // std::cout << SIMTIME << " getDistanceBetween from=" << fromEdge->getID() << " to=" << toEdge->getID() << " fromPos=" << fromPos << " toPos=" << toPos << "\n";
+    assert(fromPos >= 0. && fromPos <= fromEdge->getLength());
+    assert(toPos >= 0. && toPos <= toEdge->getLength());
+    assert(routePosition >= 0 && routePosition < (int)myEdges.size());
+    assert(routePosition == 0 || !myEdges.front()->isInternal());
+    if (fromEdge == toEdge && fromPos <= toPos) {
+        return toPos - fromPos;
     }
+    // TODO If fromEdge and toEdge are identical or both are internal and directly connected,
+    // the code does not check whether they are in any relation to the route.
     if (fromEdge->isInternal()) {
-        if (fromEdge == myEdges.front()) {
-            const MSEdge* succ = fromEdge->getSuccessors().front();
-            assert(succ != 0);
-            //std::cout << "  recurse fromSucc=" << succ->getID() << "\n";
-            return (fromEdge->getLength() - fromPos) + getDistanceBetween(0, toPos, succ, toEdge, includeInternal);
-        } else {
-            const MSEdge* pred = fromEdge->getPredecessors().front();
-            assert(pred != 0);
-            //std::cout << "  recurse fromPred=" << pred->getID() << "\n";
-            const double d = getDistanceBetween(pred->getLength(), toPos, pred, toEdge, includeInternal, routePosition) - fromPos;
-            return d < 0 ? std::numeric_limits<double>::max() : d;
-        }
+        const auto& via = fromEdge->getViaSuccessors().front();
+        const MSEdge* const succ = via.second == nullptr ? via.first : via.second;
+        assert(succ != nullptr);
+        // std::cout << "  recurse fromSucc=" << succ->getID() << "\n";
+        const double d = getDistanceBetween(0., toPos, succ, toEdge, routePosition);
+        return d == std::numeric_limits<double>::max() ? d : fromEdge->getLength() - fromPos + d;
     }
     if (toEdge->isInternal()) {
-        const MSEdge* pred = toEdge->getPredecessors().front();
-        assert(pred != 0);
-        //std::cout << "  recurse toPred=" << pred->getID() << "\n";
-        return toPos + getDistanceBetween(fromPos, pred->getLength(), fromEdge, pred, includeInternal, routePosition);
+        const MSEdge* const pred = toEdge->getPredecessors().front();
+        assert(pred != nullptr);
+        // std::cout << "  recurse toPred=" << pred->getID() << "\n";
+        const double d = getDistanceBetween(fromPos, pred->getLength(), fromEdge, pred, routePosition);
+        return d == std::numeric_limits<double>::max() ? d : toPos + d;
     }
-    ConstMSEdgeVector::const_iterator it = std::find(myEdges.begin() + routePosition, myEdges.end(), fromEdge);
-    if (it == myEdges.end() || std::find(it, myEdges.end(), toEdge) == myEdges.end()) {
-        // start or destination not contained in route
+    ConstMSEdgeVector::const_iterator fromIt = std::find(myEdges.begin() + routePosition, myEdges.end(), fromEdge);
+    if (fromIt == myEdges.end()) {
+        // start not contained in route
         return std::numeric_limits<double>::max();
     }
-    ConstMSEdgeVector::const_iterator it2 = std::find(it + 1, myEdges.end(), toEdge);
-
-    if (fromEdge == toEdge) {
-        if (fromPos <= toPos) {
-            return toPos - fromPos;
-        } else if (it2 == myEdges.end()) {
-            // we don't visit the edge again
-            return std::numeric_limits<double>::max();
-        }
+    ConstMSEdgeVector::const_iterator toIt = std::find(fromIt + 1, myEdges.end(), toEdge);
+    if (toIt == myEdges.end()) {
+        // destination not contained in route
+        return std::numeric_limits<double>::max();
     }
-    return getDistanceBetween(fromPos, toPos, it, it2, includeInternal);
+    return getDistanceBetween(fromPos, toPos, fromIt, toIt, true);
 }
 
 

src/microsim/MSRoute.h

@@ -125,22 +125,27 @@ class MSRoute : public Named, public Parameterised {
         return myEdges;
     }
 
-    /** @brief Compute the distance between 2 given edges on this route, including the length of internal lanes.
-     * Note, that for edges which contain loops:
-     * - the first occurance of fromEdge will be used
-     * - the first occurance of toEdge after the first occurance of fromEdge will be used
+    /** @brief Compute the distance between 2 given edges on this route, optionally including the length of internal lanes.
+     * Note, that for routes which contain loops:
+     * - the first occurance of fromEdge after routePosition will be used
+     * - the first occurance of toEdge after the above occurance of fromEdge will be used
+     * If fromEdge or toEdge are not in the route or the toPos on toEdge is not downstream of fromPos on fromEdge
+     * std::numeric_limits<double>::max() will be returned.
+     *
+     * The code will work with internal edges as fromEdge and toEdge and also the route may contain internal edges
+     * but only at the start or the end, not between two regular edges. If the route contains internal edges at the begin
+     * routePosition needs to be 0.
      *
      * @param[in] fromPos  position on the first edge, at wich the computed distance begins
-     * @param[in] toPos    position on the last edge, at which the coumputed distance endsance
+     * @param[in] toPos    position on the last edge, at which the computed distance ends
      * @param[in] fromEdge edge at wich computation begins
      * @param[in] toEdge   edge at which distance computation shall stop
-     * @param[in] includeInternal Whether the lengths of internal edges shall be counted
      * @param[in] routePosition Optional offset when searching for the fromEdge within the route
      * @return             distance between the position fromPos on fromEdge and toPos on toEdge
      */
-    double getDistanceBetween(double fromPos, double toPos, const MSEdge* fromEdge, const MSEdge* toEdge, bool includeInternal = true, int routePosition = 0) const;
+    double getDistanceBetween(double fromPos, double toPos, const MSEdge* fromEdge, const MSEdge* toEdge, int routePosition = 0) const;
 
-    /** @brief Compute the distance between 2 given edges on this route, including the length of internal lanes.
+    /** @brief Compute the distance between 2 given edges on this route, optionally including the length of internal lanes.
      * This has the same semantics as above but uses iterators instead of edge
      * points so looping routes are not an issue.
      *

src/microsim/output/MSFCDExport.cpp

@@ -127,7 +127,7 @@ MSFCDExport::write(OutputDevice& of, SUMOTime timestep, bool elevation) {
                     double lanePos = veh->getPositionOnLane();
                     if (microVeh != nullptr && microVeh->getLane()->isInternal()) {
                         lanePos = microVeh->getRoute().getDistanceBetween(0, lanePos, microVeh->getEdge(), &microVeh->getLane()->getEdge(),
-                                  true, microVeh->getRoutePosition());
+                                  microVeh->getRoutePosition());
                     }
                     of.writeOptionalAttr(SUMO_ATTR_DISTANCE, veh->getEdge()->getDistanceAt(lanePos), mask);
                 }