SUMO - Simulation of Urban MObility
NBContHelper.cpp
Go to the documentation of this file.
1 /****************************************************************************/
9 // Some methods for traversing lists of edges
10 /****************************************************************************/
11 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
12 // Copyright (C) 2001-2016 DLR (http://www.dlr.de/) and contributors
13 /****************************************************************************/
14 //
15 // This file is part of SUMO.
16 // SUMO is free software: you can redistribute it and/or modify
17 // it under the terms of the GNU General Public License as published by
18 // the Free Software Foundation, either version 3 of the License, or
19 // (at your option) any later version.
20 //
21 /****************************************************************************/
22 
23 
24 // ===========================================================================
25 // included modules
26 // ===========================================================================
27 #ifdef _MSC_VER
28 #include <windows_config.h>
29 #else
30 #include <config.h>
31 #endif
32 
33 #include <vector>
34 #include <map>
35 #include <cassert>
36 #include "NBContHelper.h"
37 #include <utils/geom/GeomHelper.h>
38 
39 #ifdef CHECK_MEMORY_LEAKS
40 #include <foreign/nvwa/debug_new.h>
41 #endif // CHECK_MEMORY_LEAKS
42 
43 
44 // ===========================================================================
45 // method definitions
46 // ===========================================================================
47 /* -------------------------------------------------------------------------
48  * utility methods
49  * ----------------------------------------------------------------------- */
50 void
51 NBContHelper::nextCW(const EdgeVector& edges, EdgeVector::const_iterator& from) {
52  from++;
53  if (from == edges.end()) {
54  from = edges.begin();
55  }
56 }
57 
58 
59 void
60 NBContHelper::nextCCW(const EdgeVector& edges, EdgeVector::const_iterator& from) {
61  if (from == edges.begin()) {
62  from = edges.end() - 1;
63  } else {
64  --from;
65  }
66 }
67 
68 
69 std::ostream&
70 NBContHelper::out(std::ostream& os, const std::vector<bool>& v) {
71  for (std::vector<bool>::const_iterator i = v.begin(); i != v.end(); i++) {
72  os << *i;
73  }
74  return os;
75 }
76 
77 
78 NBEdge*
80  NBNode* from, NBNode* to) {
81  for (EdgeVector::const_iterator i = edges.begin(); i != edges.end(); i++) {
82  if ((*i)->getToNode() == to && (*i)->getFromNode() == from) {
83  return *i;
84  }
85  }
86  return 0;
87 }
88 
89 
90 
93  assert(ev.size() > 0);
94  SUMOReal max = (*(ev.begin()))->getSpeed();
95  for (EdgeVector::const_iterator i = ev.begin() + 1; i != ev.end(); i++) {
96  max =
97  max > (*i)->getSpeed()
98  ? max : (*i)->getSpeed();
99  }
100  return max;
101 }
102 
103 
104 
105 /* -------------------------------------------------------------------------
106  * methods from node_with_incoming_finder
107  * ----------------------------------------------------------------------- */
109  : myEdge(e) {}
110 
111 
112 bool
114  const EdgeVector& incoming = n->getIncomingEdges();
115  return std::find(incoming.begin(), incoming.end(), myEdge) != incoming.end();
116 }
117 
118 
119 
120 /* -------------------------------------------------------------------------
121  * methods from node_with_outgoing_finder
122  * ----------------------------------------------------------------------- */
124  : myEdge(e) {}
125 
126 
127 bool
129  const EdgeVector& outgoing = n->getOutgoingEdges();
130  return std::find(outgoing.begin(), outgoing.end(), myEdge) != outgoing.end();
131 }
132 
133 
134 
135 /* -------------------------------------------------------------------------
136  * methods from edge_with_destination_finder
137  * ----------------------------------------------------------------------- */
139  : myDestinationNode(dest) {}
140 
141 
142 bool
144  return e->getToNode() == myDestinationNode;
145 }
146 
147 /* -------------------------------------------------------------------------
148  * methods from relative_outgoing_edge_sorter
149  * ----------------------------------------------------------------------- */
150 int
152  if (e1 == 0 || e2 == 0) {
153  return -1;
154  }
155  SUMOReal relAngle1 = NBHelpers::normRelAngle(
156  myEdge->getEndAngle(), e1->getStartAngle());
157  SUMOReal relAngle2 = NBHelpers::normRelAngle(
158  myEdge->getEndAngle(), e2->getStartAngle());
159 
160  SUMOReal lookAhead = 2 * NBEdge::ANGLE_LOOKAHEAD;
161  while (fabs(relAngle1 - relAngle2) < 3.0) {
162  // look at further geometry segments to resolve ambiguity
163  const Position referencePos1 = e1->getGeometry().positionAtOffset2D(lookAhead);
164  const Position referencePos2 = e2->getGeometry().positionAtOffset2D(lookAhead);
165  relAngle1 = NBHelpers::normRelAngle(myEdge->getEndAngle(), GeomHelper::legacyDegree(
166  e1->getFromNode()->getPosition().angleTo2D(referencePos1), true));
167  relAngle2 = NBHelpers::normRelAngle(myEdge->getEndAngle(), GeomHelper::legacyDegree(
168  e2->getFromNode()->getPosition().angleTo2D(referencePos2), true));
169  if (lookAhead > MAX2(e1->getLength(), e2->getLength())) {
170  break;
171  }
172  lookAhead *= 2;
173  }
174  return relAngle1 > relAngle2;
175 }
176 
177 
178 /* -------------------------------------------------------------------------
179  * methods from straightness_sorter
180  * ----------------------------------------------------------------------- */
181 int
183  if (e1 == 0 || e2 == 0) {
184  return -1;
185  }
186  SUMOReal relAngle1 = NBHelpers::normRelAngle(
187  myReferenceAngle, myRefIncoming ? e1->getShapeStartAngle() : e1->getShapeEndAngle());
188  SUMOReal relAngle2 = NBHelpers::normRelAngle(
189  myReferenceAngle, myRefIncoming ? e2->getShapeStartAngle() : e2->getShapeEndAngle());
190  const int geomIndex = myRefIncoming ? 0 : -1;
191 
192  //std::cout << " e1=" << e1->getID() << " e2=" << e2->getID() << " refA=" << myReferenceAngle << " initially a1=" << relAngle1 << " a2=" << relAngle2 << "\n";
193  const SUMOReal e1Length = e1->getGeometry().length2D();
194  const SUMOReal e2Length = e2->getGeometry().length2D();
195  const SUMOReal maxLookAhead = MAX2(e1Length, e2Length);
196  SUMOReal lookAhead = MIN2(maxLookAhead, 2 * NBEdge::ANGLE_LOOKAHEAD);
197  while (fabs(relAngle1 - relAngle2) < 3.0) {
198  // look at further geometry segments to resolve ambiguity
199  const SUMOReal offset1 = myRefIncoming ? lookAhead : e1Length - lookAhead;
200  const SUMOReal offset2 = myRefIncoming ? lookAhead : e2Length - lookAhead;
201  const Position referencePos1 = e1->getGeometry().positionAtOffset2D(offset1);
202  const Position referencePos2 = e2->getGeometry().positionAtOffset2D(offset2);
203 
204  relAngle1 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
205  e1->getGeometry()[geomIndex].angleTo2D(referencePos1), true));
206  relAngle2 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
207  e2->getGeometry()[geomIndex].angleTo2D(referencePos2), true));
208 
209  if (lookAhead > maxLookAhead) {
210  break;
211  }
212  lookAhead *= 2;
213  }
214  if (fabs(relAngle1 - relAngle2) < 3.0) {
215  // use angle to end of reference edge as tiebraker
216  relAngle1 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
217  myReferencePos.angleTo2D(e1->getLaneShape(0)[geomIndex]), true));
218  relAngle2 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
219  myReferencePos.angleTo2D(e2->getLaneShape(0)[geomIndex]), true));
220  //std::cout << " tiebraker refPos=" << myReferencePos << " abs1="
221  // << GeomHelper::legacyDegree(myReferencePos.angleTo2D(e1->getLaneShape(0).front()), true)
222  // << " abs2=" << GeomHelper::legacyDegree(myReferencePos.angleTo2D(e2->getLaneShape(0).front()), true) << "\n";
223  }
224  //std::cout << " e1=" << e1->getID() << " e2=" << e2->getID() << " a1=" << relAngle1 << " a2=" << relAngle2 << "\n";
225  return fabs(relAngle1) < fabs(relAngle2);
226 }
227 
228 
229 /* -------------------------------------------------------------------------
230  * methods from relative_incoming_edge_sorter
231  * ----------------------------------------------------------------------- */
232 int
234  if (e1 == 0 || e2 == 0) {
235  return -1;
236  }
237  SUMOReal relAngle1 = NBHelpers::normRelAngle(
238  myEdge->getStartAngle(), e1->getEndAngle());
239  SUMOReal relAngle2 = NBHelpers::normRelAngle(
240  myEdge->getStartAngle(), e2->getEndAngle());
241 
242  SUMOReal lookAhead = 2 * NBEdge::ANGLE_LOOKAHEAD;
243  while (fabs(relAngle1 - relAngle2) < 3.0) {
244  // look at further geometry segments to resolve ambiguity
245  const Position referencePos1 = e1->getGeometry().positionAtOffset2D(e1->getGeometry().length() - lookAhead);
246  const Position referencePos2 = e2->getGeometry().positionAtOffset2D(e2->getGeometry().length() - lookAhead);
247  relAngle1 = NBHelpers::normRelAngle(myEdge->getStartAngle(), GeomHelper::legacyDegree(
248  referencePos1.angleTo2D(e1->getToNode()->getPosition()), true));
249  relAngle2 = NBHelpers::normRelAngle(myEdge->getStartAngle(), GeomHelper::legacyDegree(
250  referencePos2.angleTo2D(e2->getToNode()->getPosition()), true));
251  if (lookAhead > MAX2(e1->getLength(), e2->getLength())) {
252  break;
253  }
254  lookAhead *= 2;
255  }
256  return relAngle1 > relAngle2;
257 }
258 
259 
260 std::ostream&
261 operator<<(std::ostream& os, const EdgeVector& ev) {
262  for (EdgeVector::const_iterator i = ev.begin(); i != ev.end(); i++) {
263  if (i != ev.begin()) {
264  os << ", ";
265  }
266  os << (*i)->getID();
267  }
268  return os;
269 }
270 
271 
272 
273 
274 SUMOReal
276  if (edges.size() == 0) {
277  return -1;
278  }
279  SUMOReal ret = (*(edges.begin()))->getSpeed();
280  for (EdgeVector::const_iterator i = edges.begin() + 1; i != edges.end(); i++) {
281  if ((*i)->getSpeed() > ret) {
282  ret = (*i)->getSpeed();
283  }
284  }
285  return ret;
286 }
287 
288 
289 SUMOReal
291  if (edges.size() == 0) {
292  return -1;
293  }
294  SUMOReal ret = (*(edges.begin()))->getSpeed();
295  for (EdgeVector::const_iterator i = edges.begin() + 1; i != edges.end(); i++) {
296  if ((*i)->getSpeed() < ret) {
297  ret = (*i)->getSpeed();
298  }
299  }
300  return ret;
301 }
302 
303 
304 int
306  assert(e1->getFromNode() == myNode || e1->getToNode() == myNode);
307  assert(e2->getFromNode() == myNode || e2->getToNode() == myNode);
308  const SUMOReal angle1 = e1->getAngleAtNodeToCenter(myNode);
309  const SUMOReal angle2 = e2->getAngleAtNodeToCenter(myNode);
310  const SUMOReal absDiff = fabs(angle1 - angle2);
311 
312  // cannot trust the angle difference hence a heuristic:
313  if (absDiff < 2 || absDiff > (360 - 2)) {
314  const bool sameDir = ((e1->getFromNode() == myNode && e2->getFromNode() == myNode)
315  || (e1->getToNode() == myNode && e2->getToNode() == myNode));
316  if (sameDir) {
317  // put edges that allow pedestrians on the 'outside', but be aware if both allow / disallow
318  const bool e1Peds = (e1->getPermissions() & SVC_PEDESTRIAN) != 0;
319  const bool e2Peds = (e2->getPermissions() & SVC_PEDESTRIAN) != 0;
320  if (e1->getToNode() == myNode) {
321  if (e1Peds && !e2Peds) {
322  return true;
323  } else if (!e1Peds && e2Peds) {
324  return false;
325  }
326  } else {
327  if (!e1Peds && e2Peds) {
328  return true;
329  } else if (e1Peds && !e2Peds) {
330  return false;
331  }
332  }
333  // break ties to ensure strictly weak ordering
334  return e1->getID() < e2->getID();
335  } else {
336  // sort incoming before outgoing, no need to break ties here
337  return e1->getToNode() == myNode;
338  }
339  }
340  return angle1 < angle2;
341 }
342 
343 /****************************************************************************/
344 
bool operator()(const NBNode *const n) const
SUMOReal 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:434
SUMOReal getStartAngle() const
Returns the angle at the start of the edge (relative to the node shape center) The angle is computed ...
Definition: NBEdge.h:425
node_with_outgoing_finder(const NBEdge *const e)
constructor
is a pedestrian
SUMOReal angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position ...
Definition: Position.h:243
SUMOReal getShapeStartAngle() const
Returns the angle at the start of the edge.
Definition: NBEdge.cpp:1579
SUMOReal length() const
Returns the length.
static SUMOReal normRelAngle(SUMOReal angle1, SUMOReal angle2)
ensure that reverse relAngles (>=179.999) always count as turnarounds (-180)
Definition: NBHelpers.cpp:69
friend std::ostream & operator<<(std::ostream &os, const EdgeVector &ev)
The representation of a single edge during network building.
Definition: NBEdge.h:71
int operator()(const NBEdge *e1, const NBEdge *e2) const
comparing operation
int operator()(NBEdge *e1, NBEdge *e2) const
comparing operation
T MAX2(T a, T b)
Definition: StdDefs.h:75
static void nextCW(const EdgeVector &edges, EdgeVector::const_iterator &from)
const std::string & getID() const
Returns the id.
Definition: Named.h:66
const EdgeVector & getOutgoingEdges() const
Returns this node&#39;s outgoing edges.
Definition: NBNode.h:248
#define max(a, b)
Definition: polyfonts.c:65
static std::ostream & out(std::ostream &os, const std::vector< bool > &v)
static SUMOReal legacyDegree(const SUMOReal angle, const bool positive=false)
Definition: GeomHelper.cpp:204
SUMOReal getShapeEndAngle() const
Returns the angle at the end of the edge.
Definition: NBEdge.cpp:1587
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:46
SUMOReal 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:1461
static SUMOReal maxSpeed(const EdgeVector &ev)
bool operator()(const NBNode *const n) const
T MIN2(T a, T b)
Definition: StdDefs.h:69
SUMOReal length2D() const
Returns the length.
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:2726
edge_with_destination_finder(NBNode *dest)
constructor
const PositionVector & getGeometry() const
Returns the geometry of the edge.
Definition: NBEdge.h:577
static SUMOReal getMaxSpeed(const EdgeVector &edges)
int operator()(NBEdge *e1, NBEdge *e2) const
comparing operation
const PositionVector & getLaneShape(int i) const
Returns the shape of the nth lane.
Definition: NBEdge.cpp:659
const EdgeVector & getIncomingEdges() const
Returns this node&#39;s incoming edges.
Definition: NBNode.h:240
std::vector< NBEdge * > EdgeVector
Definition: NBCont.h:41
node_with_incoming_finder(const NBEdge *const e)
constructor
const Position & getPosition() const
Returns the position of this node.
Definition: NBNode.h:228
Represents a single node (junction) during network building.
Definition: NBNode.h:74
Position positionAtOffset2D(SUMOReal pos, SUMOReal lateralOffset=0) const
Returns the position at the given length.
#define SUMOReal
Definition: config.h:213
NBNode * getFromNode() const
Returns the origin node of the edge.
Definition: NBEdge.h:409
static SUMOReal getMinSpeed(const EdgeVector &edges)
NBNode * getToNode() const
Returns the destination node of the edge.
Definition: NBEdge.h:416
static void nextCCW(const EdgeVector &edges, EdgeVector::const_iterator &from)
SUMOReal getLength() const
Returns the computed length of the edge.
Definition: NBEdge.h:463
static const SUMOReal ANGLE_LOOKAHEAD
the distance at which to take the default angle
Definition: NBEdge.h:258
static NBEdge * findConnectingEdge(const EdgeVector &edges, NBNode *from, NBNode *to)