SUMO - Simulation of Urban MObility
NBContHelper.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2018 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
17 // Some methods for traversing lists of edges
18 /****************************************************************************/
19 
20 
21 // ===========================================================================
22 // included modules
23 // ===========================================================================
24 #include <config.h>
25 
26 #include <vector>
27 #include <map>
28 #include <cassert>
29 #include "NBContHelper.h"
30 #include <utils/geom/GeomHelper.h>
31 
32 
33 // ===========================================================================
34 // method definitions
35 // ===========================================================================
36 /* -------------------------------------------------------------------------
37  * utility methods
38  * ----------------------------------------------------------------------- */
39 void
40 NBContHelper::nextCW(const EdgeVector& edges, EdgeVector::const_iterator& from) {
41  from++;
42  if (from == edges.end()) {
43  from = edges.begin();
44  }
45 }
46 
47 
48 void
49 NBContHelper::nextCCW(const EdgeVector& edges, EdgeVector::const_iterator& from) {
50  if (from == edges.begin()) {
51  from = edges.end() - 1;
52  } else {
53  --from;
54  }
55 }
56 
57 
58 std::ostream&
59 NBContHelper::out(std::ostream& os, const std::vector<bool>& v) {
60  for (std::vector<bool>::const_iterator i = v.begin(); i != v.end(); i++) {
61  os << *i;
62  }
63  return os;
64 }
65 
66 
67 NBEdge*
69  NBNode* from, NBNode* to) {
70  for (EdgeVector::const_iterator i = edges.begin(); i != edges.end(); i++) {
71  if ((*i)->getToNode() == to && (*i)->getFromNode() == from) {
72  return *i;
73  }
74  }
75  return nullptr;
76 }
77 
78 
79 
80 double
82  assert(ev.size() > 0);
83  double max = (*(ev.begin()))->getSpeed();
84  for (EdgeVector::const_iterator i = ev.begin() + 1; i != ev.end(); i++) {
85  max =
86  max > (*i)->getSpeed()
87  ? max : (*i)->getSpeed();
88  }
89  return max;
90 }
91 
92 
93 
94 /* -------------------------------------------------------------------------
95  * methods from node_with_incoming_finder
96  * ----------------------------------------------------------------------- */
98  : myEdge(e) {}
99 
100 
101 bool
103  const EdgeVector& incoming = n->getIncomingEdges();
104  return std::find(incoming.begin(), incoming.end(), myEdge) != incoming.end();
105 }
106 
107 
108 
109 /* -------------------------------------------------------------------------
110  * methods from node_with_outgoing_finder
111  * ----------------------------------------------------------------------- */
113  : myEdge(e) {}
114 
115 
116 bool
118  const EdgeVector& outgoing = n->getOutgoingEdges();
119  return std::find(outgoing.begin(), outgoing.end(), myEdge) != outgoing.end();
120 }
121 
122 
123 
124 /* -------------------------------------------------------------------------
125  * methods from edge_with_destination_finder
126  * ----------------------------------------------------------------------- */
128  : myDestinationNode(dest) {}
129 
130 
131 bool
133  return e->getToNode() == myDestinationNode;
134 }
135 
136 /* -------------------------------------------------------------------------
137  * methods from relative_outgoing_edge_sorter
138  * ----------------------------------------------------------------------- */
139 int
141  if (e1 == nullptr || e2 == nullptr) {
142  return -1;
143  }
144  double relAngle1 = NBHelpers::normRelAngle(
145  myEdge->getEndAngle(), e1->getStartAngle());
146  double relAngle2 = NBHelpers::normRelAngle(
147  myEdge->getEndAngle(), e2->getStartAngle());
148 
149  double lookAhead = 2 * NBEdge::ANGLE_LOOKAHEAD;
150  while (fabs(relAngle1 - relAngle2) < 3.0) {
151  // look at further geometry segments to resolve ambiguity
152  const Position referencePos1 = e1->getGeometry().positionAtOffset2D(lookAhead);
153  const Position referencePos2 = e2->getGeometry().positionAtOffset2D(lookAhead);
154  relAngle1 = NBHelpers::normRelAngle(myEdge->getEndAngle(), GeomHelper::legacyDegree(
155  e1->getFromNode()->getPosition().angleTo2D(referencePos1), true));
156  relAngle2 = NBHelpers::normRelAngle(myEdge->getEndAngle(), GeomHelper::legacyDegree(
157  e2->getFromNode()->getPosition().angleTo2D(referencePos2), true));
158  if (lookAhead > MAX2(e1->getLength(), e2->getLength())) {
159  break;
160  }
161  lookAhead *= 2;
162  }
163  return relAngle1 > relAngle2;
164 }
165 
166 
167 /* -------------------------------------------------------------------------
168  * methods from straightness_sorter
169  * ----------------------------------------------------------------------- */
170 int
172  if (e1 == nullptr || e2 == nullptr) {
173  return -1;
174  }
175  double relAngle1 = NBHelpers::normRelAngle(
176  myReferenceAngle, myRefIncoming ? e1->getShapeStartAngle() : e1->getShapeEndAngle());
177  double relAngle2 = NBHelpers::normRelAngle(
178  myReferenceAngle, myRefIncoming ? e2->getShapeStartAngle() : e2->getShapeEndAngle());
179  const int geomIndex = myRefIncoming ? 0 : -1;
180 
181  //std::cout << " e1=" << e1->getID() << " e2=" << e2->getID() << " refA=" << myReferenceAngle << " initially a1=" << relAngle1 << " a2=" << relAngle2 << "\n";
182  const double e1Length = e1->getGeometry().length2D();
183  const double e2Length = e2->getGeometry().length2D();
184  const double maxLookAhead = MAX2(e1Length, e2Length);
185  double lookAhead = MIN2(maxLookAhead, 2 * NBEdge::ANGLE_LOOKAHEAD);
186  while (fabs(relAngle1 - relAngle2) < 3.0) {
187  // look at further geometry segments to resolve ambiguity
188  const double offset1 = myRefIncoming ? lookAhead : e1Length - lookAhead;
189  const double offset2 = myRefIncoming ? lookAhead : e2Length - lookAhead;
190  const Position referencePos1 = e1->getGeometry().positionAtOffset2D(offset1);
191  const Position referencePos2 = e2->getGeometry().positionAtOffset2D(offset2);
192 
193  relAngle1 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
194  e1->getGeometry()[geomIndex].angleTo2D(referencePos1), true));
195  relAngle2 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
196  e2->getGeometry()[geomIndex].angleTo2D(referencePos2), true));
197 
198  if (lookAhead > maxLookAhead) {
199  break;
200  }
201  lookAhead *= 2;
202  }
203  if (fabs(relAngle1 - relAngle2) < 3.0) {
204  // use angle to end of reference edge as tiebraker
205  relAngle1 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
206  myReferencePos.angleTo2D(e1->getLaneShape(0)[geomIndex]), true));
207  relAngle2 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
208  myReferencePos.angleTo2D(e2->getLaneShape(0)[geomIndex]), true));
209  //std::cout << " tiebraker refPos=" << myReferencePos << " abs1="
210  // << GeomHelper::legacyDegree(myReferencePos.angleTo2D(e1->getLaneShape(0).front()), true)
211  // << " abs2=" << GeomHelper::legacyDegree(myReferencePos.angleTo2D(e2->getLaneShape(0).front()), true) << "\n";
212  }
213  //std::cout << " e1=" << e1->getID() << " e2=" << e2->getID() << " a1=" << relAngle1 << " a2=" << relAngle2 << "\n";
214  return fabs(relAngle1) < fabs(relAngle2);
215 }
216 
217 
218 /* -------------------------------------------------------------------------
219  * methods from relative_incoming_edge_sorter
220  * ----------------------------------------------------------------------- */
221 int
223  if (e1 == nullptr || e2 == nullptr) {
224  return -1;
225  }
226  double relAngle1 = NBHelpers::normRelAngle(
227  myEdge->getStartAngle(), e1->getEndAngle());
228  double relAngle2 = NBHelpers::normRelAngle(
229  myEdge->getStartAngle(), e2->getEndAngle());
230 
231  double lookAhead = 2 * NBEdge::ANGLE_LOOKAHEAD;
232  while (fabs(relAngle1 - relAngle2) < 3.0) {
233  // look at further geometry segments to resolve ambiguity
234  const Position referencePos1 = e1->getGeometry().positionAtOffset2D(e1->getGeometry().length() - lookAhead);
235  const Position referencePos2 = e2->getGeometry().positionAtOffset2D(e2->getGeometry().length() - lookAhead);
236  relAngle1 = NBHelpers::normRelAngle(myEdge->getStartAngle(), GeomHelper::legacyDegree(
237  referencePos1.angleTo2D(e1->getToNode()->getPosition()), true));
238  relAngle2 = NBHelpers::normRelAngle(myEdge->getStartAngle(), GeomHelper::legacyDegree(
239  referencePos2.angleTo2D(e2->getToNode()->getPosition()), true));
240  if (lookAhead > MAX2(e1->getLength(), e2->getLength())) {
241  break;
242  }
243  lookAhead *= 2;
244  }
245  return relAngle1 > relAngle2;
246 }
247 
248 
249 std::ostream&
250 operator<<(std::ostream& os, const EdgeVector& ev) {
251  for (EdgeVector::const_iterator i = ev.begin(); i != ev.end(); i++) {
252  if (i != ev.begin()) {
253  os << ", ";
254  }
255  os << (*i)->getID();
256  }
257  return os;
258 }
259 
260 
261 
262 
263 double
265  if (edges.size() == 0) {
266  return -1;
267  }
268  double ret = (*(edges.begin()))->getSpeed();
269  for (EdgeVector::const_iterator i = edges.begin() + 1; i != edges.end(); i++) {
270  if ((*i)->getSpeed() > ret) {
271  ret = (*i)->getSpeed();
272  }
273  }
274  return ret;
275 }
276 
277 
278 double
280  if (edges.size() == 0) {
281  return -1;
282  }
283  double ret = (*(edges.begin()))->getSpeed();
284  for (EdgeVector::const_iterator i = edges.begin() + 1; i != edges.end(); i++) {
285  if ((*i)->getSpeed() < ret) {
286  ret = (*i)->getSpeed();
287  }
288  }
289  return ret;
290 }
291 
292 
293 int
295  assert(e1->getFromNode() == myNode || e1->getToNode() == myNode);
296  assert(e2->getFromNode() == myNode || e2->getToNode() == myNode);
297  const double angle1 = e1->getAngleAtNodeToCenter(myNode);
298  const double angle2 = e2->getAngleAtNodeToCenter(myNode);
299  const double absDiff = fabs(angle1 - angle2);
300 
301  // cannot trust the angle difference hence a heuristic:
302  if (absDiff < 2 || absDiff > (360 - 2)) {
303  const bool sameDir = ((e1->getFromNode() == myNode && e2->getFromNode() == myNode)
304  || (e1->getToNode() == myNode && e2->getToNode() == myNode));
305  if (sameDir) {
306  // put edges that allow pedestrians on the 'outside', but be aware if both allow / disallow
307  const bool e1Peds = (e1->getPermissions() & SVC_PEDESTRIAN) != 0;
308  const bool e2Peds = (e2->getPermissions() & SVC_PEDESTRIAN) != 0;
309  if (e1->getToNode() == myNode) {
310  if (e1Peds && !e2Peds) {
311  return true;
312  } else if (!e1Peds && e2Peds) {
313  return false;
314  }
315  } else {
316  if (!e1Peds && e2Peds) {
317  return true;
318  } else if (e1Peds && !e2Peds) {
319  return false;
320  }
321  }
322  // break ties to ensure strictly weak ordering
323  return e1->getID() < e2->getID();
324  } else {
325  // sort incoming before outgoing, no need to break ties here
326  return e1->getToNode() == myNode;
327  }
328  }
329  return angle1 < angle2;
330 }
331 
332 /****************************************************************************/
333 
bool operator()(const NBNode *const n) const
double getLength() const
Returns the computed length of the edge.
Definition: NBEdge.h:488
double getAngleAtNodeToCenter(const NBNode *const node) const
Returns the angle of from the node shape center to where the edge meets the node shape.
Definition: NBEdge.cpp:1813
double length2D() const
Returns the length.
node_with_outgoing_finder(const NBEdge *const e)
constructor
static double maxSpeed(const EdgeVector &ev)
is a pedestrian
static double getMinSpeed(const EdgeVector &edges)
static double normRelAngle(double angle1, double angle2)
ensure that reverse relAngles (>=179.999) always count as turnarounds (-180)
Definition: NBHelpers.cpp:60
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
static const double ANGLE_LOOKAHEAD
the distance at which to take the default angle
Definition: NBEdge.h:279
friend std::ostream & operator<<(std::ostream &os, const EdgeVector &ev)
The representation of a single edge during network building.
Definition: NBEdge.h:65
int operator()(const NBEdge *e1, const NBEdge *e2) const
comparing operation
int operator()(NBEdge *e1, NBEdge *e2) const
comparing operation
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position ...
Definition: Position.h:254
T MAX2(T a, T b)
Definition: StdDefs.h:76
static void nextCW(const EdgeVector &edges, EdgeVector::const_iterator &from)
const std::string & getID() const
Returns the id.
Definition: Named.h:78
double getShapeStartAngle() const
Returns the angle at the start of the edge.
Definition: NBEdge.cpp:1951
static double legacyDegree(const double angle, const bool positive=false)
Definition: GeomHelper.cpp:203
const EdgeVector & getOutgoingEdges() const
Returns this node&#39;s outgoing edges (The edges which start at this node)
Definition: NBNode.h:255
static std::ostream & out(std::ostream &os, const std::vector< bool > &v)
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:39
bool operator()(const NBNode *const n) const
T MIN2(T a, T b)
Definition: StdDefs.h:70
int operator()(NBEdge *e1, NBEdge *e2) const
comparing operation
SVCPermissions getPermissions(int lane=-1) const
get the union of allowed classes over all lanes or for a specific lane
Definition: NBEdge.cpp:3331
edge_with_destination_finder(NBNode *dest)
constructor
const PositionVector & getGeometry() const
Returns the geometry of the edge.
Definition: NBEdge.h:622
int operator()(NBEdge *e1, NBEdge *e2) const
comparing operation
double length() const
Returns the length.
const PositionVector & getLaneShape(int i) const
Returns the shape of the nth lane.
Definition: NBEdge.cpp:855
const EdgeVector & getIncomingEdges() const
Returns this node&#39;s incoming edges (The edges which yield in this node)
Definition: NBNode.h:250
std::vector< NBEdge * > EdgeVector
container for (sorted) edges
Definition: NBCont.h:34
double getStartAngle() const
Returns the angle at the start of the edge (relative to the node shape center) The angle is computed ...
Definition: NBEdge.h:450
node_with_incoming_finder(const NBEdge *const e)
constructor
const Position & getPosition() const
Definition: NBNode.h:242
Represents a single node (junction) during network building.
Definition: NBNode.h:68
static double getMaxSpeed(const EdgeVector &edges)
NBNode * getFromNode() const
Returns the origin node of the edge.
Definition: NBEdge.h:434
double getShapeEndAngle() const
Returns the angle at the end of the edge.
Definition: NBEdge.cpp:1959
NBNode * getToNode() const
Returns the destination node of the edge.
Definition: NBEdge.h:441
static void nextCCW(const EdgeVector &edges, EdgeVector::const_iterator &from)
double getEndAngle() const
Returns the angle at the end of the edge (relative to the node shape center) The angle is computed in...
Definition: NBEdge.h:459
static NBEdge * findConnectingEdge(const EdgeVector &edges, NBNode *from, NBNode *to)