CARLA
 
载入中...
搜索中...
未找到
DoublyConnectedEdgeList.cpp
浏览该文件的文档.
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"
9
10#include <cmath>
11#include <map>
12
13#ifdef CARLA_ROAD_GENERATOR_EXTRA_LOG
14#include <sstream>
15#endif // CARLA_ROAD_GENERATOR_EXTRA_LOG
16
17namespace MapGen {
18
19 // ===========================================================================
20 // --局部静态方法 ---------------------------------------------------
21 // ===========================================================================
22
23#ifdef CARLA_ROAD_GENERATOR_EXTRA_LOG
24
25 static void ResetIndices() {
26 GraphNode::ResetIndex();
27 GraphHalfEdge::ResetIndex();
28 GraphFace::ResetIndex();
29 }
30
31#endif // CARLA_ROAD_GENERATOR_EXTRA_LOG
32
33 /// 返回一对 {prev, next},其中 prev/next 是围绕边的源节点逆时针方向的前一条/后一条边。即,边的位置介于 prev 和 next 之间
34 ///
35 /// 注意:始终返回从节点指向外的半边
36 ///
37 /// 时间复杂度为 O(n*log(n)),其中 n 是边的源节点的边数
38 static std::pair<DoublyConnectedEdgeList::HalfEdge *, DoublyConnectedEdgeList::HalfEdge *>
40 {
41 using Dcel = DoublyConnectedEdgeList;
42 // 从 [-π, π] 到 [0, 1]
43 auto normalize = [](auto a) {
44 constexpr float twoPi = 2.0 * 3.14159265359;
45 a /= twoPi;
46 while (a >= 1.0) a -= 1.0;
47 while (a < 0.0) a += 1.0;
48 return a;
49 };
50 auto angle = Dcel::GetAngle(halfEdge);
51 std::map<decltype(angle), Dcel::HalfEdge *> edgeMap;
52 //遍历源节点中的每一条半边
53 auto &firstHalfEdge = Dcel::GetLeavingHalfEdge(Dcel::GetSource(halfEdge));
54 auto *edge = &firstHalfEdge;
55 do {
56 if (edge != &halfEdge) {
57 auto alpha = DoublyConnectedEdgeList::GetAngle(*edge);
58 auto a = normalize(alpha - angle);
59 edgeMap.insert({a, edge});
60 }
61 edge = &Dcel::GetNextInNode(*edge);
62 } while (edge != &firstHalfEdge);
63 check(!edgeMap.empty());
64 return {edgeMap.rbegin()->second, edgeMap.begin()->second};
65 }
66
67 // ===========================================================================
68 // -- 构造函数和析构函数 --------------------------------------------
69 // ===========================================================================
70
72 const Position &Position0,
73 const Position &Position1) :
74 Nodes(),
75 HalfEdges(2u),
76 Faces(1u)
77 {
78 Nodes.emplace_back(Position0);
79 Nodes.emplace_back(Position1);
80
81 Faces.front().HalfEdge = &HalfEdges.front();
82
83 HalfEdges.front().Source = &Nodes.front();
84 HalfEdges.front().Target = &Nodes.back();
85 HalfEdges.front().Next = &HalfEdges.back();
86 HalfEdges.front().Pair = &HalfEdges.back();
87 HalfEdges.front().Face = &Faces.front();
88
89 HalfEdges.back().Source = &Nodes.back();
90 HalfEdges.back().Target = &Nodes.front();
91 HalfEdges.back().Next = &HalfEdges.front();
92 HalfEdges.back().Pair = &HalfEdges.front();
93 HalfEdges.back().Face = &Faces.front();
94
95 Nodes.front().LeavingHalfEdge = &HalfEdges.front();
96 Nodes.back().LeavingHalfEdge = &HalfEdges.back();
97 }
98
100 {
101#ifdef CARLA_ROAD_GENERATOR_EXTRA_LOG
102 ResetIndices();
103#endif // CARLA_ROAD_GENERATOR_EXTRA_LOG
104 }
105
106 // ===========================================================================
107 // -- 将元素添加到图中 -------------------------------------------
108 // ===========================================================================
109
111 const Position &NodePosition,
112 Node &OtherNode)
113 {
114 Nodes.emplace_back(NodePosition);
115 auto &newNode = Nodes.back();
116 HalfEdges.emplace_back();
117 auto &edge0 = HalfEdges.back();
118 HalfEdges.emplace_back();
119 auto &edge1 = HalfEdges.back();
120
121 edge0.Source = &newNode;
122 edge0.Target = &OtherNode;
123 edge0.Pair = &edge1;
124
125 edge1.Source = &OtherNode;
126 edge1.Target = &newNode;
127 edge1.Pair = &edge0;
128
129 HalfEdge *prev;
130 HalfEdge *next;
131 std::tie(prev, next) = FindPositionInNode(edge1);
132
133 edge0.Next = next;
134 edge0.Face = next->Face;
135
136 edge1.Next = &edge0;
137 edge1.Face = next->Face;
138
139 prev->Pair->Next = &edge1;
140
141 newNode.LeavingHalfEdge = &edge0;
142 return newNode;
143 }
144
146 const Position &Position,
147 HalfEdge &edge)
148 {
149 HalfEdges.emplace_back();
150 auto &edge0 = HalfEdges.back();
151 HalfEdges.emplace_back();
152 auto &edge1 = HalfEdges.back();
153
154 Nodes.emplace_back(Position);
155 auto &newNode = Nodes.back();
156
157 auto &node0 = *edge.Source;
158
159 //边缘的相反方向
160 edge0.Source = &newNode;
161 edge0.Target = &node0;
162 edge0.Pair = &edge1;
163 edge0.Face = edge.Pair->Face;
164
165 // 与边缘方向相同
166 edge1.Source = &node0;
167 edge1.Target = &newNode;
168 edge1.Pair = &edge0;
169 edge1.Next = &edge;
170 edge1.Face = edge.Face;
171
172 // 修复与节点0的连接
173 HalfEdge *prev;
174 HalfEdge *next;
175 std::tie(prev, next) = FindPositionInNode(edge);
176 edge0.Next = next;
177 prev->Pair->Next = &edge1;
178
179 // 修复被拆分的那一对
180 edge.Source = &newNode;
181 edge.Pair->Target = &newNode;
182 edge.Pair->Next = &edge0;
183
184 // 修复节点的边
185 node0.LeavingHalfEdge = &edge1;
186 newNode.LeavingHalfEdge = &edge0;
187
188 return newNode;
189 }
190
192 Node &Node0,
193 Node &Node1)
194 {
195 Faces.emplace_back();
196 auto &newFace = Faces.back();
197 HalfEdges.emplace_back();
198 auto &edge0 = HalfEdges.back();
199 HalfEdges.emplace_back();
200 auto &edge1 = HalfEdges.back();
201
202 edge0.Source = &Node0;
203 edge0.Target = &Node1;
204 edge0.Pair = &edge1;
205 edge1.Source = &Node1;
206 edge1.Target = &Node0;
207 edge1.Pair = &edge0;
208
209 //将边连接到节点0
210 HalfEdge *prev0;
211 HalfEdge *next0;
212 std::tie(prev0, next0) = FindPositionInNode(edge0);
213 edge1.Next = next0;
214 prev0->Pair->Next = &edge0;
215
216 //将边连接到节点1
217 HalfEdge *prev1;
218 HalfEdge *next1;
219 std::tie(prev1, next1) = FindPositionInNode(edge1);
220 edge0.Next = next1;
221 prev1->Pair->Next = &edge1;
222
223 //将面附加到新创建的边上
224 auto &oldFace = *next1->Face;
225 oldFace.HalfEdge = &edge0;
226 newFace.HalfEdge = &edge1;
227
228 // 遍历每个面的边并修正指针
229 auto fixFace = [](Face &face) {
230 auto &firstEdge = GetHalfEdge(face);
231 auto *edge = &firstEdge;
232 do {
233 edge->Face = &face;
234 edge = &GetNextInFace(*edge);
235 } while (edge != &firstEdge);
236 };
237 fixFace(oldFace);
238 fixFace(newFace);
239
240 return newFace;
241 }
242
243 // ===========================================================================
244 // --其他成员函数 -------------------------------------------------
245 // ===========================================================================
246
248 auto src = GetSource(halfEdge).GetPosition();
249 auto trg = GetTarget(halfEdge).GetPosition();
250 auto dir = trg - src; // @todo normalize?
251 return std::atan2(static_cast<double>(dir.y), static_cast<double>(dir.x));
252 }
253
254#ifdef CARLA_ROAD_GENERATOR_EXTRA_LOG
255
256 void DoublyConnectedEdgeList::PrintToLog() const
257 {
258 std::wstringstream sout;
259 {
260 sout << "iterate all nodes: ";
261 for (auto &node : GetNodes()) {
262 auto p = node.GetPosition();
263 sout << node << "{" << p.x << "," << p.y << "} ";
264 }
265 sout << "\n";
266 }
267 {
268 sout << "iterate all faces: ";
269 for (auto &face : GetFaces()) {
270 sout << face << " ";
271 }
272 sout << "\n";
273 }
274 {
275 sout << "iterate all edges: ";
276 for (auto &edge : GetHalfEdges()) {
277 auto &src = GetSource(edge);
278 auto &trg = GetTarget(edge);
279 auto &face = GetFace(edge);
280 sout << edge << "{" << src << "->" << trg << "," << face << "} ";
281 }
282 sout << "\n";
283 }
284 {
285 sout << "iterate nodes in face: ";
286 for (auto &face : GetFaces()) {
287 sout << face << "{ ";
288 auto &firstEdge = GetHalfEdge(face);
289 const auto *edge = &firstEdge;
290 do {
291 sout << GetSource(*edge) << " ";
292 edge = &GetNextInFace(*edge);
293 } while (edge != &firstEdge);
294 sout << "} ";
295 }
296 sout << "\n";
297 }
298 {
299 sout << "iterate edges in node: ";
300 for (auto &node : GetNodes()) {
301 sout << node << "{ ";
302 auto &firstEdge = GetLeavingHalfEdge(node);
303 const auto *edge = &firstEdge;
304 do {
305 sout << GetTarget(*edge) << " ";
306 edge = &GetNextInNode(*edge);
307 } while (edge != &firstEdge);
308 sout << "} ";
309 }
310 sout << "\n";
311 }
312 UE_LOG(LogCarla, Log, TEXT("\n%s"), sout.str().c_str());
313 }
314
315#endif // CARLA_ROAD_GENERATOR_EXTRA_LOG
316
317} // namespace MapGen
UE_LOG(LogCarla, Log, TEXT("UActorDispatcher::Destroying actor: '%s' %x"), *Id, Actor)
简单的双连通边链表结构。它只允许添加元素,不允许删除元素。
Node & AddNode(const Position &NodePosition, Node &OtherNode)
{
Face & ConnectNodes(Node &Node0, Node &Node1)
用一对边连接两个节点。
ListView< HalfEdgeIterator > GetHalfEdges()
DoublyConnectedEdgeList(const Position &Position0, const Position &Position1)
创建一个有2个节点、2个边和1个面的双连通边链表DoublyConnectedEdgeList。
static float GetAngle(const HalfEdge &halfEdge)
返回 half-edge 的角度,范围为 [-pi, pi]
static HalfEdge & GetHalfEdge(Face &face)
static Node & GetTarget(HalfEdge &halfEdge)
static HalfEdge & GetLeavingHalfEdge(Node &node)
static Face & GetFace(HalfEdge &halfEdge)
static Node & GetSource(HalfEdge &halfEdge)
static HalfEdge & GetNextInNode(HalfEdge &halfEdge)
static HalfEdge & GetNextInFace(HalfEdge &halfEdge)
Node & SplitEdge(const Position &Position, HalfEdge &HalfEdge)
在 位置分割 HalfEdge (和它的配对)
static std::pair< DoublyConnectedEdgeList::HalfEdge *, DoublyConnectedEdgeList::HalfEdge * > FindPositionInNode(DoublyConnectedEdgeList::HalfEdge &halfEdge)
返回一对 {prev, next},其中 prev/next 是围绕边的源节点逆时针方向的前一条/后一条边。即,边的位置介于 prev 和 next 之间
const DoublyConnectedEdgeList::Position & GetPosition() const