CARLA
GraphParser.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #include "Carla.h"
8 #include "GraphParser.h"
9 
11 
12 #include <type_traits>
13 #include <unordered_set>
14 
15 namespace MapGen {
16 
17  using Graph = DoublyConnectedEdgeList;
18 
19  // ===========================================================================
20  // -- Local static methods ---------------------------------------------------
21  // ===========================================================================
22 
23  static int getQuadrant(float angle) {
24  return static_cast<int>(std::round(angle/HALF_PI));
25  }
26 
27  // Assumes angles are separated by half pi approx.
28  static float getRotation(float angle0, float angle1) {
29  const int min = getQuadrant(std::min(angle0, angle1));
30  const int max = getQuadrant(std::max(angle0, angle1));
31  return HALF_PI * std::min(min, min * max);
32  }
33 
34  // Assumes angles are separated by half pi approx.
35  static float getRotation(float angle0, float angle1, float angle2) {
36  /// @todo There has to be a better way.
37  switch (getQuadrant(angle0) + getQuadrant(angle1) + getQuadrant(angle2)) {
38  case 0:
39  return HALF_PI;
40  case 1:
41  return 0.0;
42  case 2:
43  return -1.0 * HALF_PI;
44  case 3:
45  return PI;
46  default:
47  UE_LOG(LogCarla, Error, TEXT("Wrong quadrants"));
48  return 0.0;
49  }
50  }
51 
52  /// @todo This can probably be done at graph creation.
53  static void fixGraphData(Graph &graph) {
54  // Set the edge count for each node in the graph.
55  for (auto &node : graph.GetNodes()) {
56  std::vector<float> angles;
57  angles.reserve(4u);
58  // Iterate every half-edge in this node.
59  auto &firstEdge = Graph::GetLeavingHalfEdge(node);
60  auto *edge = &firstEdge;
61  do {
62  edge->Angle = Graph::GetAngle(*edge);
63  angles.emplace_back(edge->Angle);
64  edge = &Graph::GetNextInNode(*edge);
65  } while (edge != &firstEdge);
66  check(!angles.empty());
67  node.EdgeCount = angles.size();
68  node.bIsIntersection = true;
69  switch (node.EdgeCount) {
70  case 2:
71  node.Rotation = getRotation(angles[0u], angles[1u]);
72  node.IntersectionType = EIntersectionType::Turn90Deg;
73  break;
74  case 3:
75  node.Rotation = getRotation(angles[0u], angles[1u], angles[2u]);
76  node.IntersectionType = EIntersectionType::TIntersection;
77  break;
78  case 4:
79  default:
80  node.Rotation = 0.0;
81  node.IntersectionType = EIntersectionType::XIntersection;
82  break;
83  }
84  node.Rots.swap(angles);
85  }
86  }
87 
88  // ===========================================================================
89  // -- RoadSegmentBuilder -----------------------------------------------------
90  // ===========================================================================
91 
93  public:
94 
95  std::vector<TUniquePtr<RoadSegmentDescription>> Segments;
96 
97  explicit RoadSegmentBuilder(const Graph &graph) : _graph(graph) {}
98 
99  void Add(Graph::HalfEdge &edge) {
100  if (!insert(edge))
101  return;
102  if (Graph::GetSource(edge).bIsIntersection) {
103  Segments.emplace_back(MakeUnique<RoadSegmentDescription>());
104  _handlingInitial = false;
105  }
106  if (_handlingInitial) {
107  _initial.emplace_back(&edge);
108  } else {
109  Segments.back()->Add(edge);
110  }
111  }
112 
113  void Close() {
114  for (auto edge : _initial) {
115  Segments.back()->Add(*edge);
116  }
117  _handlingInitial = true;
118  }
119 
120  private:
121 
122  /// Insert both half-edges only if they haven't been visited yet.
123  bool insert(Graph::HalfEdge &edge) {
124  return _visitedEdges.insert(&edge).second &&
125  _visitedEdges.insert(&Graph::GetPair(edge)).second;
126  }
127 
128  const Graph &_graph;
129 
130  std::unordered_set<const Graph::HalfEdge *> _visitedEdges;
131 
132  bool _handlingInitial = true;
133 
134  std::vector<const Graph::HalfEdge *> _initial;
135  };
136 
137  // ===========================================================================
138  // -- GraphParser ------------------------------------------------------------
139  // ===========================================================================
140 
142  check(graph.CountNodes() >= 4u);
143  check(graph.CountHalfEdges() >= 8u);
144  check(graph.CountFaces() >= 2u);
145 
146  fixGraphData(graph);
147 
148  CityAreas.reserve(graph.CountFaces() - 1);
149 
150  RoadSegmentBuilder rsb(graph);
151 
152  auto faceList = graph.GetFaces();
153  auto it = faceList.begin();
154  ++it; // Ignore first face (unbounded).
155  for (; it != faceList.end(); ++it) {
156  CityAreas.emplace_back(MakeUnique<CityAreaDescription>(*it));
157  CityAreaDescription &cityArea = *CityAreas.back();
158  // Iterate every half-edge in this face.
159  auto &firstEdge = Graph::GetHalfEdge(*it);
160  for (auto *edge = &Graph::GetNextInFace(firstEdge);
161  edge != &firstEdge;
162  edge = &Graph::GetNextInFace(*edge)) {
163  cityArea.Add(Graph::GetSource(*edge));
164  rsb.Add(*edge);
165  }
166  rsb.Close();
167  }
168 
169  RoadSegments.swap(rsb.Segments);
170  }
171 
172 } // namespace MapGen
static HalfEdge & GetHalfEdge(Face &face)
static float GetAngle(const HalfEdge &halfEdge)
Return the angle [-pi, pi] of the half-edge.
Simple doubly-connected edge list structure.
ListView< NodeIterator > GetNodes()
bool insert(Graph::HalfEdge &edge)
Insert both half-edges only if they haven&#39;t been visited yet.
static Node & GetSource(HalfEdge &halfEdge)
void Add(Graph::HalfEdge &edge)
Definition: GraphParser.cpp:99
static HalfEdge & GetPair(HalfEdge &halfEdge)
static float getRotation(float angle0, float angle1)
Definition: GraphParser.cpp:28
static int getQuadrant(float angle)
Definition: GraphParser.cpp:23
RoadSegmentBuilder(const Graph &graph)
Definition: GraphParser.cpp:97
std::vector< const Graph::HalfEdge * > _initial
DoublyConnectedEdgeList Graph
static HalfEdge & GetLeavingHalfEdge(Node &node)
std::vector< TUniquePtr< RoadSegmentDescription > > Segments
Definition: GraphParser.cpp:95
void Add(const GraphNode &Node)
double min(double v1, double v2)
Definition: Simplify.h:294
static HalfEdge & GetNextInFace(HalfEdge &halfEdge)
static void fixGraphData(Graph &graph)
Definition: GraphParser.cpp:53
static HalfEdge & GetNextInNode(HalfEdge &halfEdge)
ListView< FaceIterator > GetFaces()
std::unordered_set< const Graph::HalfEdge * > _visitedEdges
GraphParser(DoublyConnectedEdgeList &Dcel)