Line data    Source code

       1             : /****************************************************************************/
       2             : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
       3             : // Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
       4             : // This program and the accompanying materials are made available under the
       5             : // terms of the Eclipse Public License 2.0 which is available at
       6             : // https://www.eclipse.org/legal/epl-2.0/
       7             : // This Source Code may also be made available under the following Secondary
       8             : // Licenses when the conditions for such availability set forth in the Eclipse
       9             : // Public License 2.0 are satisfied: GNU General Public License, version 2
      10             : // or later which is available at
      11             : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
      12             : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
      13             : /****************************************************************************/
      14             : /// @file    NBContHelper.cpp
      15             : /// @author  Daniel Krajzewicz
      16             : /// @author  Jakob Erdmann
      17             : /// @author  Michael Behrisch
      18             : /// @date    Tue, 20 Nov 2001
      19             : ///
      20             : // Some methods for traversing lists of edges
      21             : /****************************************************************************/
      22             : #include <config.h>
      23             : 
      24             : #include <vector>
      25             : #include <map>
      26             : #include <cassert>
      27             : #include "NBContHelper.h"
      28             : #include <utils/geom/GeomHelper.h>
      29             : 
      30             : 
      31             : // ===========================================================================
      32             : // method definitions
      33             : // ===========================================================================
      34             : /* -------------------------------------------------------------------------
      35             :  * utility methods
      36             :  * ----------------------------------------------------------------------- */
      37             : void
      38     6234404 : NBContHelper::nextCW(const EdgeVector& edges, EdgeVector::const_iterator& from) {
      39             :     from++;
      40     6234404 :     if (from == edges.end()) {
      41     1006576 :         from = edges.begin();
      42             :     }
      43     6234404 : }
      44             : 
      45             : 
      46             : void
      47     5835985 : NBContHelper::nextCCW(const EdgeVector& edges, EdgeVector::const_iterator& from) {
      48     5835985 :     if (from == edges.begin()) {
      49      715438 :         from = edges.end() - 1;
      50             :     } else {
      51             :         --from;
      52             :     }
      53     5835985 : }
      54             : 
      55             : 
      56             : std::ostream&
      57           0 : NBContHelper::out(std::ostream& os, const std::vector<bool>& v) {
      58           0 :     for (std::vector<bool>::const_iterator i = v.begin(); i != v.end(); i++) {
      59           0 :         os << *i;
      60             :     }
      61           0 :     return os;
      62             : }
      63             : 
      64             : 
      65             : NBEdge*
      66           0 : NBContHelper::findConnectingEdge(const EdgeVector& edges,
      67             :                                  NBNode* from, NBNode* to) {
      68           0 :     for (EdgeVector::const_iterator i = edges.begin(); i != edges.end(); i++) {
      69           0 :         if ((*i)->getToNode() == to && (*i)->getFromNode() == from) {
      70             :             return *i;
      71             :         }
      72             :     }
      73             :     return nullptr;
      74             : }
      75             : 
      76             : 
      77             : 
      78             : double
      79        3650 : NBContHelper::maxSpeed(const EdgeVector& ev) {
      80             :     assert(ev.size() > 0);
      81        3650 :     double max = (*(ev.begin()))->getSpeed();
      82       14896 :     for (EdgeVector::const_iterator i = ev.begin() + 1; i != ev.end(); i++) {
      83             :         max =
      84       11246 :             max > (*i)->getSpeed()
      85       11246 :             ? max : (*i)->getSpeed();
      86             :     }
      87        3650 :     return max;
      88             : }
      89             : 
      90             : 
      91             : 
      92             : /* -------------------------------------------------------------------------
      93             :  * methods from node_with_incoming_finder
      94             :  * ----------------------------------------------------------------------- */
      95     1187169 : NBContHelper::node_with_incoming_finder::node_with_incoming_finder(const NBEdge* const e)
      96     1187169 :     : myEdge(e) {}
      97             : 
      98             : 
      99             : bool
     100     2957441 : NBContHelper::node_with_incoming_finder::operator()(const NBNode* const n) const {
     101             :     const EdgeVector& incoming = n->getIncomingEdges();
     102     2957441 :     return std::find(incoming.begin(), incoming.end(), myEdge) != incoming.end();
     103             : }
     104             : 
     105             : 
     106             : 
     107             : /* -------------------------------------------------------------------------
     108             :  * methods from node_with_outgoing_finder
     109             :  * ----------------------------------------------------------------------- */
     110     1187908 : NBContHelper::node_with_outgoing_finder::node_with_outgoing_finder(const NBEdge* const e)
     111     1187908 :     : myEdge(e) {}
     112             : 
     113             : 
     114             : bool
     115     2968897 : NBContHelper::node_with_outgoing_finder::operator()(const NBNode* const n) const {
     116             :     const EdgeVector& outgoing = n->getOutgoingEdges();
     117     2968897 :     return std::find(outgoing.begin(), outgoing.end(), myEdge) != outgoing.end();
     118             : }
     119             : 
     120             : 
     121             : /* -------------------------------------------------------------------------
     122             :  * methods from edge_with_destination_finder
     123             :  * ----------------------------------------------------------------------- */
     124           0 : NBContHelper::edge_with_destination_finder::edge_with_destination_finder(NBNode* dest)
     125           0 :     : myDestinationNode(dest) {}
     126             : 
     127             : 
     128             : bool
     129           0 : NBContHelper::edge_with_destination_finder::operator()(NBEdge* e) const {
     130           0 :     return e->getToNode() == myDestinationNode;
     131             : }
     132             : 
     133             : 
     134             : /* -------------------------------------------------------------------------
     135             :  * methods from relative_outgoing_edge_sorter
     136             :  * ----------------------------------------------------------------------- */
     137             : bool
     138      264536 : NBContHelper::relative_outgoing_edge_sorter::operator()(const NBEdge* e1, const NBEdge* e2) const {
     139             :     assert(e1 != nullptr && e2 != nullptr);
     140      264536 :     double relAngle1 = NBHelpers::normRelAngle(myAngle, e1->getStartAngle());
     141      264536 :     double relAngle2 = NBHelpers::normRelAngle(myAngle, e2->getStartAngle());
     142      264536 :     const double length1 = e1->getGeometry().length2D();
     143      264536 :     const double length2 = e2->getGeometry().length2D();
     144             : 
     145      264536 :     double lookAhead = 2 * NBEdge::ANGLE_LOOKAHEAD;
     146      265033 :     while (fabs(relAngle1 - relAngle2) < 3.0) {
     147             :         // look at further geometry segments to resolve ambiguity
     148             :         const double offset1 = MAX2(0.0, MIN2(length1, lookAhead));
     149             :         const double offset2 = MAX2(0.0, MIN2(length2, lookAhead));
     150         801 :         const Position referencePos1 = e1->getGeometry().positionAtOffset2D(offset1);
     151         801 :         const Position referencePos2 = e2->getGeometry().positionAtOffset2D(offset2);
     152         801 :         const double angle1 = GeomHelper::legacyDegree(e1->getFromNode()->getPosition().angleTo2D(referencePos1), true);
     153         801 :         const double angle2 = GeomHelper::legacyDegree(e2->getFromNode()->getPosition().angleTo2D(referencePos2), true);
     154         801 :         relAngle1 = NBHelpers::normRelAngle(myAngle, angle1);
     155         801 :         relAngle2 = NBHelpers::normRelAngle(myAngle, angle2);
     156         801 :         if (lookAhead > MAX2(length1, length2)) {
     157             :             break;
     158             :         }
     159         497 :         lookAhead *= 2;
     160             :     }
     161      264536 :     if (fabs(relAngle1 - relAngle2) < NUMERICAL_EPS) {
     162             :         // need to break ties for windows debug version, numerical id may be -1 for both
     163         201 :         return e1->getID() > e2->getID();
     164             :     }
     165      264335 :     return relAngle1 > relAngle2;
     166             : }
     167             : 
     168             : 
     169             : /* -------------------------------------------------------------------------
     170             :  * methods from relative_incoming_edge_sorter
     171             :  * ----------------------------------------------------------------------- */
     172             : bool
     173        4703 : NBContHelper::relative_incoming_edge_sorter::operator()(const NBEdge* e1, const NBEdge* e2) const {
     174             :     assert(e1 != nullptr && e2 != nullptr);
     175        4703 :     double relAngle1 = NBHelpers::normRelAngle(myAngle, e1->getEndAngle());
     176        4703 :     double relAngle2 = NBHelpers::normRelAngle(myAngle, e2->getEndAngle());
     177        4703 :     const double length1 = e1->getGeometry().length2D();
     178        4703 :     const double length2 = e2->getGeometry().length2D();
     179             : 
     180        4703 :     double lookAhead = 2 * NBEdge::ANGLE_LOOKAHEAD;
     181        4779 :     while (fabs(relAngle1 - relAngle2) < 3.0) {
     182             :         // look at further geometry segments to resolve ambiguity
     183          83 :         const double offset1 = MAX2(0.0, MIN2(length1, length1 - lookAhead));
     184          83 :         const double offset2 = MAX2(0.0, MIN2(length2, length2 - lookAhead));
     185          83 :         const Position referencePos1 = e1->getGeometry().positionAtOffset2D(offset1);
     186          83 :         const Position referencePos2 = e2->getGeometry().positionAtOffset2D(offset2);
     187          83 :         const double angle1 = GeomHelper::legacyDegree(referencePos1.angleTo2D(e1->getToNode()->getPosition()), true);
     188          83 :         const double angle2 = GeomHelper::legacyDegree(referencePos2.angleTo2D(e2->getToNode()->getPosition()), true);
     189          83 :         relAngle1 = NBHelpers::normRelAngle(myAngle, angle1);
     190          83 :         relAngle2 = NBHelpers::normRelAngle(myAngle, angle2);
     191          83 :         if (lookAhead > MAX2(length1, length2)) {
     192             :             break;
     193             :         }
     194          76 :         lookAhead *= 2;
     195             :     }
     196        4703 :     if (fabs(relAngle1 - relAngle2) < NUMERICAL_EPS) {
     197             :         // need to break ties for windows debug version, numerical id may be -1 for both
     198           4 :         return e1->getID() > e2->getID();
     199             :     }
     200        4699 :     return relAngle1 > relAngle2;
     201             : }
     202             : 
     203             : 
     204             : std::ostream&
     205           0 : operator<<(std::ostream& os, const EdgeVector& ev) {
     206           0 :     for (EdgeVector::const_iterator i = ev.begin(); i != ev.end(); i++) {
     207           0 :         if (i != ev.begin()) {
     208           0 :             os << ", ";
     209             :         }
     210           0 :         os << (*i)->getID();
     211             :     }
     212           0 :     return os;
     213             : }
     214             : 
     215             : 
     216             : double
     217           0 : NBContHelper::getMaxSpeed(const EdgeVector& edges) {
     218           0 :     if (edges.size() == 0) {
     219             :         return -1;
     220             :     }
     221           0 :     double ret = (*(edges.begin()))->getSpeed();
     222           0 :     for (EdgeVector::const_iterator i = edges.begin() + 1; i != edges.end(); i++) {
     223           0 :         if ((*i)->getSpeed() > ret) {
     224           0 :             ret = (*i)->getSpeed();
     225             :         }
     226             :     }
     227             :     return ret;
     228             : }
     229             : 
     230             : 
     231             : double
     232           0 : NBContHelper::getMinSpeed(const EdgeVector& edges) {
     233           0 :     if (edges.size() == 0) {
     234             :         return -1;
     235             :     }
     236           0 :     double ret = (*(edges.begin()))->getSpeed();
     237           0 :     for (EdgeVector::const_iterator i = edges.begin() + 1; i != edges.end(); i++) {
     238           0 :         if ((*i)->getSpeed() < ret) {
     239           0 :             ret = (*i)->getSpeed();
     240             :         }
     241             :     }
     242             :     return ret;
     243             : }
     244             : 
     245             : 
     246             : bool
     247     3298267 : NBContHelper::edge_by_angle_to_nodeShapeCentroid_sorter::operator()(const NBEdge* e1, const NBEdge* e2) const {
     248             :     assert(e1->getFromNode() == myNode || e1->getToNode() == myNode);
     249             :     assert(e2->getFromNode() == myNode || e2->getToNode() == myNode);
     250     3298267 :     const double angle1 = e1->getAngleAtNodeToCenter(myNode);
     251     3298267 :     const double angle2 = e2->getAngleAtNodeToCenter(myNode);
     252     3298267 :     const double absDiff = fabs(angle1 - angle2);
     253             : 
     254             :     // cannot trust the angle difference hence a heuristic:
     255     3298267 :     if (absDiff < 2 || absDiff > (360 - 2)) {
     256      320426 :         const bool sameDir = ((e1->getFromNode() == myNode && e2->getFromNode() == myNode)
     257      698044 :                               || (e1->getToNode() == myNode && e2->getToNode() == myNode));
     258             :         if (sameDir) {
     259             :             // put edges that allow pedestrians on the 'outside', but be aware if both allow / disallow
     260       11707 :             const bool e1Peds = (e1->getPermissions() & SVC_PEDESTRIAN) != 0;
     261       11707 :             const bool e2Peds = (e2->getPermissions() & SVC_PEDESTRIAN) != 0;
     262       11707 :             if (e1->getToNode() == myNode) {
     263        6457 :                 if (e1Peds && !e2Peds) {
     264             :                     return true;
     265        5799 :                 } else if (!e1Peds && e2Peds) {
     266             :                     return false;
     267             :                 }
     268             :             } else {
     269        5250 :                 if (!e1Peds && e2Peds) {
     270             :                     return true;
     271        4756 :                 } else if (e1Peds && !e2Peds) {
     272             :                     return false;
     273             :                 }
     274             :             }
     275             :             // break ties to ensure strictly weak ordering
     276        7938 :             return e1->getID() < e2->getID();
     277             :         } else {
     278             :             // sort incoming before outgoing, no need to break ties here
     279      371161 :             return e1->getToNode() == myNode;
     280             :         }
     281             :     }
     282     2915399 :     return angle1 < angle2;
     283             : }
     284             : 
     285             : 
     286             : /****************************************************************************/