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 
40 // ===========================================================================
41 // method definitions
42 // ===========================================================================
43 /* -------------------------------------------------------------------------
44  * utility methods
45  * ----------------------------------------------------------------------- */
46 void
47 NBContHelper::nextCW(const EdgeVector& edges, EdgeVector::const_iterator& from) {
48  from++;
49  if (from == edges.end()) {
50  from = edges.begin();
51  }
52 }
53 
54 
55 void
56 NBContHelper::nextCCW(const EdgeVector& edges, EdgeVector::const_iterator& from) {
57  if (from == edges.begin()) {
58  from = edges.end() - 1;
59  } else {
60  --from;
61  }
62 }
63 
64 
65 std::ostream&
66 NBContHelper::out(std::ostream& os, const std::vector<bool>& v) {
67  for (std::vector<bool>::const_iterator i = v.begin(); i != v.end(); i++) {
68  os << *i;
69  }
70  return os;
71 }
72 
73 
74 NBEdge*
76  NBNode* from, NBNode* to) {
77  for (EdgeVector::const_iterator i = edges.begin(); i != edges.end(); i++) {
78  if ((*i)->getToNode() == to && (*i)->getFromNode() == from) {
79  return *i;
80  }
81  }
82  return 0;
83 }
84 
85 
86 
87 double
89  assert(ev.size() > 0);
90  double max = (*(ev.begin()))->getSpeed();
91  for (EdgeVector::const_iterator i = ev.begin() + 1; i != ev.end(); i++) {
92  max =
93  max > (*i)->getSpeed()
94  ? max : (*i)->getSpeed();
95  }
96  return max;
97 }
98 
99 
100 
101 /* -------------------------------------------------------------------------
102  * methods from node_with_incoming_finder
103  * ----------------------------------------------------------------------- */
105  : myEdge(e) {}
106 
107 
108 bool
110  const EdgeVector& incoming = n->getIncomingEdges();
111  return std::find(incoming.begin(), incoming.end(), myEdge) != incoming.end();
112 }
113 
114 
115 
116 /* -------------------------------------------------------------------------
117  * methods from node_with_outgoing_finder
118  * ----------------------------------------------------------------------- */
120  : myEdge(e) {}
121 
122 
123 bool
125  const EdgeVector& outgoing = n->getOutgoingEdges();
126  return std::find(outgoing.begin(), outgoing.end(), myEdge) != outgoing.end();
127 }
128 
129 
130 
131 /* -------------------------------------------------------------------------
132  * methods from edge_with_destination_finder
133  * ----------------------------------------------------------------------- */
135  : myDestinationNode(dest) {}
136 
137 
138 bool
140  return e->getToNode() == myDestinationNode;
141 }
142 
143 /* -------------------------------------------------------------------------
144  * methods from relative_outgoing_edge_sorter
145  * ----------------------------------------------------------------------- */
146 int
148  if (e1 == 0 || e2 == 0) {
149  return -1;
150  }
151  double relAngle1 = NBHelpers::normRelAngle(
152  myEdge->getEndAngle(), e1->getStartAngle());
153  double relAngle2 = NBHelpers::normRelAngle(
154  myEdge->getEndAngle(), e2->getStartAngle());
155 
156  double lookAhead = 2 * NBEdge::ANGLE_LOOKAHEAD;
157  while (fabs(relAngle1 - relAngle2) < 3.0) {
158  // look at further geometry segments to resolve ambiguity
159  const Position referencePos1 = e1->getGeometry().positionAtOffset2D(lookAhead);
160  const Position referencePos2 = e2->getGeometry().positionAtOffset2D(lookAhead);
161  relAngle1 = NBHelpers::normRelAngle(myEdge->getEndAngle(), GeomHelper::legacyDegree(
162  e1->getFromNode()->getPosition().angleTo2D(referencePos1), true));
163  relAngle2 = NBHelpers::normRelAngle(myEdge->getEndAngle(), GeomHelper::legacyDegree(
164  e2->getFromNode()->getPosition().angleTo2D(referencePos2), true));
165  if (lookAhead > MAX2(e1->getLength(), e2->getLength())) {
166  break;
167  }
168  lookAhead *= 2;
169  }
170  return relAngle1 > relAngle2;
171 }
172 
173 
174 /* -------------------------------------------------------------------------
175  * methods from straightness_sorter
176  * ----------------------------------------------------------------------- */
177 int
179  if (e1 == 0 || e2 == 0) {
180  return -1;
181  }
182  double relAngle1 = NBHelpers::normRelAngle(
183  myReferenceAngle, myRefIncoming ? e1->getShapeStartAngle() : e1->getShapeEndAngle());
184  double relAngle2 = NBHelpers::normRelAngle(
185  myReferenceAngle, myRefIncoming ? e2->getShapeStartAngle() : e2->getShapeEndAngle());
186  const int geomIndex = myRefIncoming ? 0 : -1;
187 
188  //std::cout << " e1=" << e1->getID() << " e2=" << e2->getID() << " refA=" << myReferenceAngle << " initially a1=" << relAngle1 << " a2=" << relAngle2 << "\n";
189  const double e1Length = e1->getGeometry().length2D();
190  const double e2Length = e2->getGeometry().length2D();
191  const double maxLookAhead = MAX2(e1Length, e2Length);
192  double lookAhead = MIN2(maxLookAhead, 2 * NBEdge::ANGLE_LOOKAHEAD);
193  while (fabs(relAngle1 - relAngle2) < 3.0) {
194  // look at further geometry segments to resolve ambiguity
195  const double offset1 = myRefIncoming ? lookAhead : e1Length - lookAhead;
196  const double offset2 = myRefIncoming ? lookAhead : e2Length - lookAhead;
197  const Position referencePos1 = e1->getGeometry().positionAtOffset2D(offset1);
198  const Position referencePos2 = e2->getGeometry().positionAtOffset2D(offset2);
199 
200  relAngle1 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
201  e1->getGeometry()[geomIndex].angleTo2D(referencePos1), true));
202  relAngle2 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
203  e2->getGeometry()[geomIndex].angleTo2D(referencePos2), true));
204 
205  if (lookAhead > maxLookAhead) {
206  break;
207  }
208  lookAhead *= 2;
209  }
210  if (fabs(relAngle1 - relAngle2) < 3.0) {
211  // use angle to end of reference edge as tiebraker
212  relAngle1 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
213  myReferencePos.angleTo2D(e1->getLaneShape(0)[geomIndex]), true));
214  relAngle2 = NBHelpers::normRelAngle(myReferenceAngle, GeomHelper::legacyDegree(
215  myReferencePos.angleTo2D(e2->getLaneShape(0)[geomIndex]), true));
216  //std::cout << " tiebraker refPos=" << myReferencePos << " abs1="
217  // << GeomHelper::legacyDegree(myReferencePos.angleTo2D(e1->getLaneShape(0).front()), true)
218  // << " abs2=" << GeomHelper::legacyDegree(myReferencePos.angleTo2D(e2->getLaneShape(0).front()), true) << "\n";
219  }
220  //std::cout << " e1=" << e1->getID() << " e2=" << e2->getID() << " a1=" << relAngle1 << " a2=" << relAngle2 << "\n";
221  return fabs(relAngle1) < fabs(relAngle2);
222 }
223 
224 
225 /* -------------------------------------------------------------------------
226  * methods from relative_incoming_edge_sorter
227  * ----------------------------------------------------------------------- */
228 int
230  if (e1 == 0 || e2 == 0) {
231  return -1;
232  }
233  double relAngle1 = NBHelpers::normRelAngle(
234  myEdge->getStartAngle(), e1->getEndAngle());
235  double relAngle2 = NBHelpers::normRelAngle(
236  myEdge->getStartAngle(), e2->getEndAngle());
237 
238  double lookAhead = 2 * NBEdge::ANGLE_LOOKAHEAD;
239  while (fabs(relAngle1 - relAngle2) < 3.0) {
240  // look at further geometry segments to resolve ambiguity
241  const Position referencePos1 = e1->getGeometry().positionAtOffset2D(e1->getGeometry().length() - lookAhead);
242  const Position referencePos2 = e2->getGeometry().positionAtOffset2D(e2->getGeometry().length() - lookAhead);
243  relAngle1 = NBHelpers::normRelAngle(myEdge->getStartAngle(), GeomHelper::legacyDegree(
244  referencePos1.angleTo2D(e1->getToNode()->getPosition()), true));
245  relAngle2 = NBHelpers::normRelAngle(myEdge->getStartAngle(), GeomHelper::legacyDegree(
246  referencePos2.angleTo2D(e2->getToNode()->getPosition()), true));
247  if (lookAhead > MAX2(e1->getLength(), e2->getLength())) {
248  break;
249  }
250  lookAhead *= 2;
251  }
252  return relAngle1 > relAngle2;
253 }
254 
255 
256 std::ostream&
257 operator<<(std::ostream& os, const EdgeVector& ev) {
258  for (EdgeVector::const_iterator i = ev.begin(); i != ev.end(); i++) {
259  if (i != ev.begin()) {
260  os << ", ";
261  }
262  os << (*i)->getID();
263  }
264  return os;
265 }
266 
267 
268 
269 
270 double
272  if (edges.size() == 0) {
273  return -1;
274  }
275  double ret = (*(edges.begin()))->getSpeed();
276  for (EdgeVector::const_iterator i = edges.begin() + 1; i != edges.end(); i++) {
277  if ((*i)->getSpeed() > ret) {
278  ret = (*i)->getSpeed();
279  }
280  }
281  return ret;
282 }
283 
284 
285 double
287  if (edges.size() == 0) {
288  return -1;
289  }
290  double ret = (*(edges.begin()))->getSpeed();
291  for (EdgeVector::const_iterator i = edges.begin() + 1; i != edges.end(); i++) {
292  if ((*i)->getSpeed() < ret) {
293  ret = (*i)->getSpeed();
294  }
295  }
296  return ret;
297 }
298 
299 
300 int
302  assert(e1->getFromNode() == myNode || e1->getToNode() == myNode);
303  assert(e2->getFromNode() == myNode || e2->getToNode() == myNode);
304  const double angle1 = e1->getAngleAtNodeToCenter(myNode);
305  const double angle2 = e2->getAngleAtNodeToCenter(myNode);
306  const double absDiff = fabs(angle1 - angle2);
307 
308  // cannot trust the angle difference hence a heuristic:
309  if (absDiff < 2 || absDiff > (360 - 2)) {
310  const bool sameDir = ((e1->getFromNode() == myNode && e2->getFromNode() == myNode)
311  || (e1->getToNode() == myNode && e2->getToNode() == myNode));
312  if (sameDir) {
313  // put edges that allow pedestrians on the 'outside', but be aware if both allow / disallow
314  const bool e1Peds = (e1->getPermissions() & SVC_PEDESTRIAN) != 0;
315  const bool e2Peds = (e2->getPermissions() & SVC_PEDESTRIAN) != 0;
316  if (e1->getToNode() == myNode) {
317  if (e1Peds && !e2Peds) {
318  return true;
319  } else if (!e1Peds && e2Peds) {
320  return false;
321  }
322  } else {
323  if (!e1Peds && e2Peds) {
324  return true;
325  } else if (e1Peds && !e2Peds) {
326  return false;
327  }
328  }
329  // break ties to ensure strictly weak ordering
330  return e1->getID() < e2->getID();
331  } else {
332  // sort incoming before outgoing, no need to break ties here
333  return e1->getToNode() == myNode;
334  }
335  }
336  return angle1 < angle2;
337 }
338 
339 /****************************************************************************/
340 
bool operator()(const NBNode *const n) const
double getLength() const
Returns the computed length of the edge.
Definition: NBEdge.h:481
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:1588
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:65
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:276
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
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:260
T MAX2(T a, T b)
Definition: StdDefs.h:70
static void nextCW(const EdgeVector &edges, EdgeVector::const_iterator &from)
const std::string & getID() const
Returns the id.
Definition: Named.h:66
double getShapeStartAngle() const
Returns the angle at the start of the edge.
Definition: NBEdge.cpp:1715
static double legacyDegree(const double angle, const bool positive=false)
Definition: GeomHelper.cpp:200
const EdgeVector & getOutgoingEdges() const
Returns this node&#39;s outgoing edges (The edges which start at this node)
Definition: NBNode.h:245
#define max(a, b)
Definition: polyfonts.c:65
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:46
bool operator()(const NBNode *const n) const
T MIN2(T a, T b)
Definition: StdDefs.h:64
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:2913
edge_with_destination_finder(NBNode *dest)
constructor
const PositionVector & getGeometry() const
Returns the geometry of the edge.
Definition: NBEdge.h:595
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:757
const EdgeVector & getIncomingEdges() const
Returns this node&#39;s incoming edges (The edges which yield in this node)
Definition: NBNode.h:240
std::vector< NBEdge * > EdgeVector
container for (sorted) edges
Definition: NBCont.h:41
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:443
node_with_incoming_finder(const NBEdge *const e)
constructor
const Position & getPosition() const
Definition: NBNode.h:232
Represents a single node (junction) during network building.
Definition: NBNode.h:75
static double getMaxSpeed(const EdgeVector &edges)
NBNode * getFromNode() const
Returns the origin node of the edge.
Definition: NBEdge.h:427
double getShapeEndAngle() const
Returns the angle at the end of the edge.
Definition: NBEdge.cpp:1723
NBNode * getToNode() const
Returns the destination node of the edge.
Definition: NBEdge.h:434
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:452
static NBEdge * findConnectingEdge(const EdgeVector &edges, NBNode *from, NBNode *to)