CARLA
Loading...
Searching...
No Matches
MeshFactory.cpp
Go to the documentation of this file.
1// Copyright (c) 2026 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/road/MeshFactory.h>
8
9#include <vector>
10
11#include <carla/geom/Vector3D.h>
12#include <carla/geom/Rtree.h>
13#include <carla/road/element/LaneMarking.h>
14#include <carla/road/element/RoadInfoMarkRecord.h>
15#include <carla/road/Map.h>
16#include <carla/road/Deformation.h>
17
18namespace carla {
19namespace geom {
20
21 MeshFactory::MeshFactory(rpc::OpendriveGenerationParameters params) {
22 road_param.resolution = static_cast<float>(params.vertex_distance);
23 road_param.max_road_len = static_cast<float>(params.max_road_length);
24 road_param.extra_lane_width = static_cast<float>(params.additional_width);
25 road_param.wall_height = static_cast<float>(params.wall_height);
26 road_param.vertex_width_resolution = static_cast<float>(params.vertex_width_resolution);
27 }
28
29 /// We use this epsilon to shift the waypoints away from the edges of the lane
30 /// sections to avoid floating point precision errors.
31 static constexpr double EPSILON = 10.0 * std::numeric_limits<double>::epsilon();
32 static constexpr double MESH_EPSILON = 50.0 * std::numeric_limits<double>::epsilon();
33
34 std::unique_ptr<Mesh> MeshFactory::Generate(const road::Road &road) const {
35 Mesh out_mesh;
36 for (auto &&lane_section : road.GetLaneSections()) {
37 out_mesh += *Generate(lane_section);
38 }
39 return std::make_unique<Mesh>(out_mesh);
40 }
41
42 std::unique_ptr<Mesh> MeshFactory::Generate(const road::LaneSection &lane_section) const {
43 Mesh out_mesh;
44 for (auto &&lane_pair : lane_section.GetLanes()) {
45 out_mesh += *Generate(lane_pair.second);
46 }
47 return std::make_unique<Mesh>(out_mesh);
48 }
49
50 std::unique_ptr<Mesh> MeshFactory::Generate(const road::Lane &lane) const {
51 const double s_start = lane.GetDistance() + EPSILON;
52 const double s_end = lane.GetDistance() + lane.GetLength() - EPSILON;
53 return Generate(lane, s_start, s_end);
54 }
55
56 std::unique_ptr<Mesh> MeshFactory::GenerateTesselated(const road::Lane& lane) const {
57 const double s_start = lane.GetDistance() + EPSILON;
58 const double s_end = lane.GetDistance() + lane.GetLength() - EPSILON;
59 return GenerateTesselated(lane, s_start, s_end);
60 }
61
62 std::unique_ptr<Mesh> MeshFactory::Generate(
63 const road::Lane &lane, const double s_start, const double s_end) const {
64 RELEASE_ASSERT(road_param.resolution > 0.0);
65 DEBUG_ASSERT(s_start >= 0.0);
66 DEBUG_ASSERT(s_end <= lane.GetDistance() + lane.GetLength());
67 DEBUG_ASSERT(s_end >= EPSILON);
68 DEBUG_ASSERT(s_start < s_end);
69 // The lane with lane_id 0 have no physical representation in OpenDRIVE
70 Mesh out_mesh;
71 if (lane.GetId() == 0) {
72 return std::make_unique<Mesh>(out_mesh);
73 }
74 double s_current = s_start;
75
76 std::vector<geom::Vector3D> vertices;
77 if (lane.IsStraight()) {
78 // Mesh optimization: If the lane is straight just add vertices at the
79 // begining and at the end of it
80 const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
81 vertices.push_back(edges.first);
82 vertices.push_back(edges.second);
83 } else {
84 // Iterate over the lane's 's' and store the vertices based on it's width
85 do {
86 // Get the location of the edges of the current lane at the current waypoint
87 const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
88 vertices.push_back(edges.first);
89 vertices.push_back(edges.second);
90
91 // Update the current waypoint's "s"
92 s_current += road_param.resolution;
93 } while(s_current < s_end);
94 }
95
96 // This ensures the mesh is constant and have no gaps between roads,
97 // adding geometry at the very end of the lane
98 if (s_end - (s_current - road_param.resolution) > EPSILON) {
99 const auto edges = lane.GetCornerPositions(s_end - MESH_EPSILON, road_param.extra_lane_width);
100 vertices.push_back(edges.first);
101 vertices.push_back(edges.second);
102 }
103
104 // Add the adient material, create the strip and close the material
105 out_mesh.AddMaterial(
106 lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road");
107 out_mesh.AddTriangleStrip(vertices);
108 out_mesh.EndMaterial();
109 return std::make_unique<Mesh>(out_mesh);
110 }
111
112 std::unique_ptr<Mesh> MeshFactory::GenerateTesselated(
113 const road::Lane& lane, const double s_start, const double s_end) const {
114 RELEASE_ASSERT(road_param.resolution > 0.0);
115 DEBUG_ASSERT(s_start >= 0.0);
116 DEBUG_ASSERT(s_end <= lane.GetDistance() + lane.GetLength());
117 DEBUG_ASSERT(s_end >= EPSILON);
118 DEBUG_ASSERT(s_start < s_end);
119 // The lane with lane_id 0 have no physical representation in OpenDRIVE
120 Mesh out_mesh;
121 if (lane.GetId() == 0) {
122 return std::make_unique<Mesh>(out_mesh);
123 }
124 double s_current = s_start;
125
126 std::vector<geom::Vector3D> vertices;
127 // Ensure minimum vertices in width are two
128 const size_t vertices_in_width = road_param.vertex_width_resolution >= 2 ? static_cast<size_t>(road_param.vertex_width_resolution) : size_t{2};
129 const size_t segments_number = vertices_in_width - 1;
130
131 std::vector<geom::Vector2D> uvs;
132 int uvx = 0;
133 int uvy = 0;
134 // Iterate over the lane's 's' and store the vertices based on it's width
135 do {
136 // Get the location of the edges of the current lane at the current waypoint
137 std::pair<geom::Vector3D, geom::Vector3D> edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
138 const geom::Vector3D segments_size = ( edges.second - edges.first ) / static_cast<float>(segments_number);
139 geom::Vector3D current_vertex = edges.first;
140 uvx = 0;
141 for (size_t i = 0; i < vertices_in_width; ++i) {
142 uvs.push_back(geom::Vector2D(static_cast<float>(uvx), static_cast<float>(uvy)));
143 vertices.push_back(current_vertex);
144 current_vertex = current_vertex + segments_size;
145 uvx++;
146 }
147 uvy++;
148 // Update the current waypoint's "s"
149 s_current += road_param.resolution;
150 } while (s_current < s_end);
151
152 // This ensures the mesh is constant and have no gaps between roads,
153 // adding geometry at the very end of the lane
154
155 if (s_end - (s_current - road_param.resolution) > EPSILON) {
156 std::pair<carla::geom::Vector3D, carla::geom::Vector3D> edges =
157 lane.GetCornerPositions(s_end - MESH_EPSILON, road_param.extra_lane_width);
158 const geom::Vector3D segments_size = (edges.second - edges.first) / static_cast<float>(segments_number);
159 geom::Vector3D current_vertex = edges.first;
160 uvx = 0;
161 for (size_t i = 0; i < vertices_in_width; ++i)
162 {
163 uvs.push_back(geom::Vector2D(static_cast<float>(uvx), static_cast<float>(uvy)));
164 vertices.push_back(current_vertex);
165 current_vertex = current_vertex + segments_size;
166 uvx++;
167 }
168 }
169 out_mesh.AddVertices(vertices);
170 out_mesh.AddUVs(uvs);
171
172 // Add the adient material, create the strip and close the material
173 out_mesh.AddMaterial(
174 lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road");
175
176 const size_t number_of_rows = (vertices.size() / vertices_in_width);
177
178 for (size_t i = 0; i < (number_of_rows - 1); ++i) {
179 for (size_t j = 0; j < vertices_in_width - 1; ++j) {
180 out_mesh.AddIndex( j + ( i * vertices_in_width ) + 1);
181 out_mesh.AddIndex( ( j + 1 ) + ( i * vertices_in_width ) + 1);
182 out_mesh.AddIndex( j + ( ( i + 1 ) * vertices_in_width ) + 1);
183
184 out_mesh.AddIndex( ( j + 1 ) + ( i * vertices_in_width ) + 1);
185 out_mesh.AddIndex( ( j + 1 ) + ( ( i + 1 ) * vertices_in_width ) + 1);
186 out_mesh.AddIndex( j + ( ( i + 1 ) * vertices_in_width ) + 1);
187 }
188 }
189 out_mesh.EndMaterial();
190 return std::make_unique<Mesh>(out_mesh);
191 }
192
193
194 void MeshFactory::GenerateLaneSectionOrdered(
195 const road::LaneSection &lane_section,
196 std::map<carla::road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>>& result) const {
197
198 const size_t vertices_in_width = road_param.vertex_width_resolution >= 2 ? static_cast<size_t>(road_param.vertex_width_resolution) : size_t{2};
199 std::vector<size_t> redirections;
200 for (auto &&lane_pair : lane_section.GetLanes()) {
201 auto it = std::find(redirections.begin(), redirections.end(), static_cast<size_t>(lane_pair.first));
202 if ( it == redirections.end() ) {
203 redirections.push_back(static_cast<size_t>(lane_pair.first));
204 it = std::find(redirections.begin(), redirections.end(), static_cast<size_t>(lane_pair.first));
205 }
206 size_t PosToAdd = static_cast<size_t>(it - redirections.begin());
207
208 Mesh out_mesh;
209 if(lane_pair.second.GetType() == road::Lane::LaneType::Driving ){
210 out_mesh += *GenerateTesselated(lane_pair.second);
211 }else{
212 out_mesh += *GenerateSidewalk(lane_pair.second);
213 }
214
215 if( result[lane_pair.second.GetType()].size() <= PosToAdd ){
216 result[lane_pair.second.GetType()].push_back(std::make_unique<Mesh>(out_mesh));
217 } else {
218 uint32_t verticesinwidth = 0;
219 if(lane_pair.second.GetType() == road::Lane::LaneType::Driving) {
220 verticesinwidth = static_cast<uint32_t>(vertices_in_width);
221 }else if(lane_pair.second.GetType() == road::Lane::LaneType::Sidewalk){
222 verticesinwidth = 6;
223 }else{
224 verticesinwidth = 2;
225 }
226 (result[lane_pair.second.GetType()][PosToAdd])->ConcatMesh(out_mesh, static_cast<int>(verticesinwidth));
227 }
228 }
229 }
230
231
232 std::unique_ptr<Mesh> MeshFactory::GenerateSidewalk(const road::LaneSection &lane_section) const{
233 Mesh out_mesh;
234 for (auto &&lane_pair : lane_section.GetLanes()) {
235 const double s_start = lane_pair.second.GetDistance() + EPSILON;
236 const double s_end = lane_pair.second.GetDistance() + lane_pair.second.GetLength() - EPSILON;
237 out_mesh += *GenerateSidewalk(lane_pair.second, s_start, s_end);
238 }
239 return std::make_unique<Mesh>(out_mesh);
240 }
241 std::unique_ptr<Mesh> MeshFactory::GenerateSidewalk(const road::Lane &lane) const{
242 const double s_start = lane.GetDistance() + EPSILON;
243 const double s_end = lane.GetDistance() + lane.GetLength() - EPSILON;
244 return GenerateSidewalk(lane, s_start, s_end);
245 }
246 std::unique_ptr<Mesh> MeshFactory::GenerateSidewalk(
247 const road::Lane &lane, const double s_start,
248 const double s_end ) const {
249
250 RELEASE_ASSERT(road_param.resolution > 0.0);
251 DEBUG_ASSERT(s_start >= 0.0);
252 DEBUG_ASSERT(s_end <= lane.GetDistance() + lane.GetLength());
253 DEBUG_ASSERT(s_end >= EPSILON);
254 DEBUG_ASSERT(s_start < s_end);
255 // The lane with lane_id 0 have no physical representation in OpenDRIVE
256 Mesh out_mesh;
257 if (lane.GetId() == 0) {
258 return std::make_unique<Mesh>(out_mesh);
259 }
260 double s_current = s_start;
261
262 std::vector<geom::Vector3D> vertices;
263 // Ensure minimum vertices in width are two
264 const size_t vertices_in_width = 6;
265 std::vector<geom::Vector2D> uvs;
266 int uvy = 0;
267
268 // Iterate over the lane's 's' and store the vertices based on it's width
269 do {
270 // Get the location of the edges of the current lane at the current waypoint
271 std::pair<geom::Vector3D, geom::Vector3D> edges =
272 lane.GetCornerPositions(s_current, road_param.extra_lane_width);
273
274 geom::Vector3D low_vertex_first = edges.first - geom::Vector3D(0,0,1);
275 geom::Vector3D low_vertex_second = edges.second - geom::Vector3D(0,0,1);
276 vertices.push_back(low_vertex_first);
277 uvs.push_back(geom::Vector2D(0.0f, static_cast<float>(uvy)));
278
279 vertices.push_back(edges.first);
280 uvs.push_back(geom::Vector2D(1.0f, static_cast<float>(uvy)));
281
282 vertices.push_back(edges.first);
283 uvs.push_back(geom::Vector2D(1.0f, static_cast<float>(uvy)));
284
285 vertices.push_back(edges.second);
286 uvs.push_back(geom::Vector2D(2.0f, static_cast<float>(uvy)));
287
288 vertices.push_back(edges.second);
289 uvs.push_back(geom::Vector2D(2.0f, static_cast<float>(uvy)));
290
291 vertices.push_back(low_vertex_second);
292 uvs.push_back(geom::Vector2D(3.0f, static_cast<float>(uvy)));
293
294 // Update the current waypoint's "s"
295 s_current += road_param.resolution;
296 uvy++;
297 } while (s_current < s_end);
298
299 // This ensures the mesh is constant and have no gaps between roads,
300 // adding geometry at the very end of the lane
301
302 if (s_end - (s_current - road_param.resolution) > EPSILON) {
303 std::pair<carla::geom::Vector3D, carla::geom::Vector3D> edges =
304 lane.GetCornerPositions(s_end - MESH_EPSILON, road_param.extra_lane_width);
305
306 geom::Vector3D low_vertex_first = edges.first - geom::Vector3D(0,0,1);
307 geom::Vector3D low_vertex_second = edges.second - geom::Vector3D(0,0,1);
308
309 vertices.push_back(low_vertex_first);
310 uvs.push_back(geom::Vector2D(0.0f, static_cast<float>(uvy)));
311
312 vertices.push_back(edges.first);
313 uvs.push_back(geom::Vector2D(1.0f, static_cast<float>(uvy)));
314
315 vertices.push_back(edges.first);
316 uvs.push_back(geom::Vector2D(1.0f, static_cast<float>(uvy)));
317
318 vertices.push_back(edges.second);
319 uvs.push_back(geom::Vector2D(2.0f, static_cast<float>(uvy)));
320
321 vertices.push_back(edges.second);
322 uvs.push_back(geom::Vector2D(2.0f, static_cast<float>(uvy)));
323
324 vertices.push_back(low_vertex_second);
325 uvs.push_back(geom::Vector2D(3.0f, static_cast<float>(uvy)));
326
327 }
328
329 out_mesh.AddVertices(vertices);
330 out_mesh.AddUVs(uvs);
331 // Add the adient material, create the strip and close the material
332 out_mesh.AddMaterial(
333 lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road");
334
335 const size_t number_of_rows = (vertices.size() / vertices_in_width);
336
337 for (size_t i = 0; i < (number_of_rows - 1); ++i) {
338 for (size_t j = 0; j < vertices_in_width - 1; ++j) {
339
340 if(j == 1 || j == 3){
341 continue;
342 }
343
344 out_mesh.AddIndex( j + ( i * vertices_in_width ) + 1);
345 out_mesh.AddIndex( ( j + 1 ) + ( i * vertices_in_width ) + 1);
346 out_mesh.AddIndex( j + ( ( i + 1 ) * vertices_in_width ) + 1);
347
348 out_mesh.AddIndex( ( j + 1 ) + ( i * vertices_in_width ) + 1);
349 out_mesh.AddIndex( ( j + 1 ) + ( ( i + 1 ) * vertices_in_width ) + 1);
350 out_mesh.AddIndex( j + ( ( i + 1 ) * vertices_in_width ) + 1);
351
352 }
353 }
354 out_mesh.EndMaterial();
355 return std::make_unique<Mesh>(out_mesh);
356 }
357 std::unique_ptr<Mesh> MeshFactory::GenerateWalls(const road::LaneSection &lane_section) const {
358 Mesh out_mesh;
359
360 const auto min_lane = lane_section.GetLanes().begin()->first == 0 ?
361 1 : lane_section.GetLanes().begin()->first;
362 const auto max_lane = lane_section.GetLanes().rbegin()->first == 0 ?
363 -1 : lane_section.GetLanes().rbegin()->first;
364
365 for (auto &&lane_pair : lane_section.GetLanes()) {
366 const auto &lane = lane_pair.second;
367 const double s_start = lane.GetDistance() + EPSILON;
368 const double s_end = lane.GetDistance() + lane.GetLength() - EPSILON;
369 if (lane.GetId() == max_lane) {
370 out_mesh += *GenerateLeftWall(lane, s_start, s_end);
371 }
372 if (lane.GetId() == min_lane) {
373 out_mesh += *GenerateRightWall(lane, s_start, s_end);
374 }
375 }
376 return std::make_unique<Mesh>(out_mesh);
377 }
378
379 std::unique_ptr<Mesh> MeshFactory::GenerateRightWall(
380 const road::Lane &lane, const double s_start, const double s_end) const {
381 RELEASE_ASSERT(road_param.resolution > 0.0);
382 DEBUG_ASSERT(s_start >= 0.0);
383 DEBUG_ASSERT(s_end <= lane.GetDistance() + lane.GetLength());
384 DEBUG_ASSERT(s_end >= EPSILON);
385 DEBUG_ASSERT(s_start < s_end);
386 // The lane with lane_id 0 have no physical representation in OpenDRIVE
387 Mesh out_mesh;
388 if (lane.GetId() == 0) {
389 return std::make_unique<Mesh>(out_mesh);
390 }
391 double s_current = s_start;
392 const geom::Vector3D height_vector = geom::Vector3D(0.f, 0.f, road_param.wall_height);
393
394 std::vector<geom::Vector3D> r_vertices;
395 if (lane.IsStraight()) {
396 // Mesh optimization: If the lane is straight just add vertices at the
397 // begining and at the end of it
398 const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
399 r_vertices.push_back(edges.first + height_vector);
400 r_vertices.push_back(edges.first);
401 } else {
402 // Iterate over the lane's 's' and store the vertices based on it's width
403 do {
404 // Get the location of the edges of the current lane at the current waypoint
405 const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
406 r_vertices.push_back(edges.first + height_vector);
407 r_vertices.push_back(edges.first);
408
409 // Update the current waypoint's "s"
410 s_current += road_param.resolution;
411 } while(s_current < s_end);
412 }
413
414 // This ensures the mesh is constant and have no gaps between roads,
415 // adding geometry at the very end of the lane
416 if (s_end - (s_current - road_param.resolution) > EPSILON) {
417 const auto edges = lane.GetCornerPositions(s_end - MESH_EPSILON, road_param.extra_lane_width);
418 r_vertices.push_back(edges.first + height_vector);
419 r_vertices.push_back(edges.first);
420 }
421
422 // Add the adient material, create the strip and close the material
423 out_mesh.AddMaterial(
424 lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road");
425 out_mesh.AddTriangleStrip(r_vertices);
426 out_mesh.EndMaterial();
427 return std::make_unique<Mesh>(out_mesh);
428 }
429
430 std::unique_ptr<Mesh> MeshFactory::GenerateLeftWall(
431 const road::Lane &lane, const double s_start, const double s_end) const {
432 RELEASE_ASSERT(road_param.resolution > 0.0);
433 DEBUG_ASSERT(s_start >= 0.0);
434 DEBUG_ASSERT(s_end <= lane.GetDistance() + lane.GetLength());
435 DEBUG_ASSERT(s_end >= EPSILON);
436 DEBUG_ASSERT(s_start < s_end);
437 // The lane with lane_id 0 have no physical representation in OpenDRIVE
438 Mesh out_mesh;
439 if (lane.GetId() == 0) {
440 return std::make_unique<Mesh>(out_mesh);
441 }
442 double s_current = s_start;
443 const geom::Vector3D height_vector = geom::Vector3D(0.f, 0.f, road_param.wall_height);
444
445 std::vector<geom::Vector3D> l_vertices;
446 if (lane.IsStraight()) {
447 // Mesh optimization: If the lane is straight just add vertices at the
448 // begining and at the end of it
449 const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
450 l_vertices.push_back(edges.second);
451 l_vertices.push_back(edges.second + height_vector);
452 } else {
453 // Iterate over the lane's 's' and store the vertices based on it's width
454 do {
455 // Get the location of the edges of the current lane at the current waypoint
456 const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width);
457 l_vertices.push_back(edges.second);
458 l_vertices.push_back(edges.second + height_vector);
459
460 // Update the current waypoint's "s"
461 s_current += road_param.resolution;
462 } while(s_current < s_end);
463 }
464
465 // This ensures the mesh is constant and have no gaps between roads,
466 // adding geometry at the very end of the lane
467 if (s_end - (s_current - road_param.resolution) > EPSILON) {
468 const auto edges = lane.GetCornerPositions(s_end - MESH_EPSILON, road_param.extra_lane_width);
469 l_vertices.push_back(edges.second);
470 l_vertices.push_back(edges.second + height_vector);
471 }
472
473 // Add the adient material, create the strip and close the material
474 out_mesh.AddMaterial(
475 lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road");
476 out_mesh.AddTriangleStrip(l_vertices);
477 out_mesh.EndMaterial();
478 return std::make_unique<Mesh>(out_mesh);
479 }
480
481 std::vector<std::unique_ptr<Mesh>> MeshFactory::GenerateWithMaxLen(
482 const road::Road &road) const {
483 std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
484 for (auto &&lane_section : road.GetLaneSections()) {
485 auto section_uptr_list = GenerateWithMaxLen(lane_section);
486 mesh_uptr_list.insert(
487 mesh_uptr_list.end(),
488 std::make_move_iterator(section_uptr_list.begin()),
489 std::make_move_iterator(section_uptr_list.end()));
490 }
491 return mesh_uptr_list;
492 }
493
494 std::vector<std::unique_ptr<Mesh>> MeshFactory::GenerateWithMaxLen(
495 const road::LaneSection &lane_section) const {
496 std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
497 if (lane_section.GetLength() < road_param.max_road_len) {
498 mesh_uptr_list.emplace_back(Generate(lane_section));
499 } else {
500 double s_current = lane_section.GetDistance() + EPSILON;
501 const double s_end = lane_section.GetDistance() + lane_section.GetLength() - EPSILON;
502 while(s_current + road_param.max_road_len < s_end) {
503 const auto s_until = s_current + road_param.max_road_len;
504 Mesh lane_section_mesh;
505 for (auto &&lane_pair : lane_section.GetLanes()) {
506 lane_section_mesh += *Generate(lane_pair.second, s_current, s_until);
507 }
508 mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
509 s_current = s_until;
510 }
511 if (s_end - s_current > EPSILON) {
512 Mesh lane_section_mesh;
513 for (auto &&lane_pair : lane_section.GetLanes()) {
514 lane_section_mesh += *Generate(lane_pair.second, s_current, s_end);
515 }
516 mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
517 }
518 }
519 return mesh_uptr_list;
520 }
521
522std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>> MeshFactory::GenerateOrderedWithMaxLen(
523 const road::Road &road) const {
524 std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>> mesh_uptr_list;
525 for (auto &&lane_section : road.GetLaneSections()) {
526 std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>> section_uptr_list = GenerateOrderedWithMaxLen(lane_section);
527 mesh_uptr_list.insert(
528 std::make_move_iterator(section_uptr_list.begin()),
529 std::make_move_iterator(section_uptr_list.end()));
530 }
531 return mesh_uptr_list;
532 }
533
534 std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>> MeshFactory::GenerateOrderedWithMaxLen(
535 const road::LaneSection &lane_section) const {
536 const size_t vertices_in_width = road_param.vertex_width_resolution >= 2 ? static_cast<size_t>(road_param.vertex_width_resolution) : size_t{2};
537 std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>> mesh_uptr_list;
538
539 if (lane_section.GetLength() < road_param.max_road_len) {
540 GenerateLaneSectionOrdered(lane_section, mesh_uptr_list);
541 } else {
542 double s_current = lane_section.GetDistance() + EPSILON;
543 const double s_end = lane_section.GetDistance() + lane_section.GetLength() - EPSILON;
544 std::vector<size_t> redirections;
545 while(s_current + road_param.max_road_len < s_end) {
546 const auto s_until = s_current + road_param.max_road_len;
547
548 for (auto &&lane_pair : lane_section.GetLanes()) {
549 Mesh lane_section_mesh;
550 if(lane_pair.second.GetType() == road::Lane::LaneType::Driving ){
551 lane_section_mesh += *GenerateTesselated(lane_pair.second, s_current, s_until);
552 }else{
553 lane_section_mesh += *GenerateSidewalk(lane_pair.second, s_current, s_until);
554 }
555
556 auto it = std::find(redirections.begin(), redirections.end(), static_cast<size_t>(lane_pair.first));
557 if (it == redirections.end()) {
558 redirections.push_back(static_cast<size_t>(lane_pair.first));
559 it = std::find(redirections.begin(), redirections.end(), static_cast<size_t>(lane_pair.first));
560 }
561
562 size_t PosToAdd = static_cast<size_t>(it - redirections.begin());
563 if (mesh_uptr_list[lane_pair.second.GetType()].size() <= PosToAdd) {
564 mesh_uptr_list[lane_pair.second.GetType()].push_back(std::make_unique<Mesh>(lane_section_mesh));
565 } else {
566 uint32_t verticesinwidth = 0;
567 if(lane_pair.second.GetType() == road::Lane::LaneType::Driving) {
568 verticesinwidth = static_cast<uint32_t>(vertices_in_width);
569 }else if(lane_pair.second.GetType() == road::Lane::LaneType::Sidewalk){
570 verticesinwidth = 6;
571 }else{
572 verticesinwidth = 2;
573 }
574 (mesh_uptr_list[lane_pair.second.GetType()][PosToAdd])->ConcatMesh(lane_section_mesh, static_cast<int>(verticesinwidth));
575 }
576 }
577 s_current = s_until;
578 }
579 if (s_end - s_current > EPSILON) {
580 for (auto &&lane_pair : lane_section.GetLanes()) {
581 Mesh lane_section_mesh;
582 if(lane_pair.second.GetType() == road::Lane::LaneType::Driving ){
583 lane_section_mesh += *GenerateTesselated(lane_pair.second, s_current, s_end);
584 }else{
585 lane_section_mesh += *GenerateSidewalk(lane_pair.second, s_current, s_end);
586 }
587
588 auto it = std::find(redirections.begin(), redirections.end(), static_cast<size_t>(lane_pair.first));
589 if (it == redirections.end()) {
590 redirections.push_back(static_cast<size_t>(lane_pair.first));
591 it = std::find(redirections.begin(), redirections.end(), static_cast<size_t>(lane_pair.first));
592 }
593
594 size_t PosToAdd = static_cast<size_t>(it - redirections.begin());
595
596 if (mesh_uptr_list[lane_pair.second.GetType()].size() <= PosToAdd) {
597 mesh_uptr_list[lane_pair.second.GetType()].push_back(std::make_unique<Mesh>(lane_section_mesh));
598 } else {
599 uint32_t verticesinwidth = 0;
600 if(lane_pair.second.GetType() == road::Lane::LaneType::Driving) {
601 verticesinwidth = static_cast<uint32_t>(vertices_in_width);
602 }else if(lane_pair.second.GetType() == road::Lane::LaneType::Sidewalk){
603 verticesinwidth = 6;
604 }else{
605 verticesinwidth = 2;
606 }
607 *(mesh_uptr_list[lane_pair.second.GetType()][PosToAdd]) += lane_section_mesh;
608 }
609 }
610 }
611 }
612 return mesh_uptr_list;
613 }
614
615 std::vector<std::unique_ptr<Mesh>> MeshFactory::GenerateWallsWithMaxLen(
616 const road::Road &road) const {
617 std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
618 for (auto &&lane_section : road.GetLaneSections()) {
619 auto section_uptr_list = GenerateWallsWithMaxLen(lane_section);
620 mesh_uptr_list.insert(
621 mesh_uptr_list.end(),
622 std::make_move_iterator(section_uptr_list.begin()),
623 std::make_move_iterator(section_uptr_list.end()));
624 }
625 return mesh_uptr_list;
626 }
627
628 std::vector<std::unique_ptr<Mesh>> MeshFactory::GenerateWallsWithMaxLen(
629 const road::LaneSection &lane_section) const {
630 std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
631
632 const auto min_lane = lane_section.GetLanes().begin()->first == 0 ?
633 1 : lane_section.GetLanes().begin()->first;
634 const auto max_lane = lane_section.GetLanes().rbegin()->first == 0 ?
635 -1 : lane_section.GetLanes().rbegin()->first;
636
637 if (lane_section.GetLength() < road_param.max_road_len) {
638 mesh_uptr_list.emplace_back(GenerateWalls(lane_section));
639 } else {
640 double s_current = lane_section.GetDistance() + EPSILON;
641 const double s_end = lane_section.GetDistance() + lane_section.GetLength() - EPSILON;
642 while(s_current + road_param.max_road_len < s_end) {
643 const auto s_until = s_current + road_param.max_road_len;
644 Mesh lane_section_mesh;
645 for (auto &&lane_pair : lane_section.GetLanes()) {
646 const auto &lane = lane_pair.second;
647 if (lane.GetId() == max_lane) {
648 lane_section_mesh += *GenerateLeftWall(lane, s_current, s_until);
649 }
650 if (lane.GetId() == min_lane) {
651 lane_section_mesh += *GenerateRightWall(lane, s_current, s_until);
652 }
653 }
654 mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
655 s_current = s_until;
656 }
657 if (s_end - s_current > EPSILON) {
658 Mesh lane_section_mesh;
659 for (auto &&lane_pair : lane_section.GetLanes()) {
660 const auto &lane = lane_pair.second;
661 if (lane.GetId() == max_lane) {
662 lane_section_mesh += *GenerateLeftWall(lane, s_current, s_end);
663 }
664 if (lane.GetId() == min_lane) {
665 lane_section_mesh += *GenerateRightWall(lane, s_current, s_end);
666 }
667 }
668 mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
669 }
670 }
671 return mesh_uptr_list;
672 }
673
674 std::vector<std::unique_ptr<Mesh>> MeshFactory::GenerateAllWithMaxLen(
675 const road::Road &road) const {
676 std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
677
678 // Get road meshes
679 auto roads = GenerateWithMaxLen(road);
680 mesh_uptr_list.insert(
681 mesh_uptr_list.end(),
682 std::make_move_iterator(roads.begin()),
683 std::make_move_iterator(roads.end()));
684
685 // Get wall meshes only if is not a junction
686 if (!road.IsJunction()) {
687 auto walls = GenerateWallsWithMaxLen(road);
688
689 if (roads.size() == walls.size()) {
690 for (size_t i = 0; i < walls.size(); ++i) {
691 *mesh_uptr_list[i] += *walls[i];
692 }
693 } else {
694 mesh_uptr_list.insert(
695 mesh_uptr_list.end(),
696 std::make_move_iterator(walls.begin()),
697 std::make_move_iterator(walls.end()));
698 }
699 }
700
701 return mesh_uptr_list;
702 }
703
704 void MeshFactory::GenerateAllOrderedWithMaxLen(
705 const road::Road &road,
706 std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>>& roads
707 ) const {
708
709 // Get road meshes
710 std::map<road::Lane::LaneType , std::vector<std::unique_ptr<Mesh>>> result = GenerateOrderedWithMaxLen(road);
711 for (auto &pair_map : result)
712 {
713 std::vector<std::unique_ptr<Mesh>>& origin = roads[pair_map.first];
714 std::vector<std::unique_ptr<Mesh>>& source = pair_map.second;
715 std::move(source.begin(), source.end(), std::back_inserter(origin));
716 }
717 }
718
719 void MeshFactory::GenerateLaneMarkForRoad(
720 const road::Road& road,
721 std::vector<std::unique_ptr<Mesh>>& inout,
722 std::vector<std::string>& outinfo ) const
723 {
724 for (auto&& lane_section : road.GetLaneSections()) {
725 for (auto&& lane : lane_section.GetLanes()) {
726 if (lane.first != 0) {
727 if(lane.second.GetType() == road::Lane::LaneType::Driving ){
728 GenerateLaneMarksForNotCenterLine(lane_section, lane.second, inout, outinfo);
729 outinfo.push_back("white");
730 }
731 } else {
732 if(lane.second.GetType() == road::Lane::LaneType::None ){
733 GenerateLaneMarksForCenterLine(road, lane_section, lane.second, inout, outinfo);
734 outinfo.push_back("yellow");
735 }
736 }
737 }
738 }
739 }
740
741 void MeshFactory::GenerateLaneMarksForNotCenterLine(
742 const road::LaneSection& lane_section,
743 const road::Lane& lane,
744 std::vector<std::unique_ptr<Mesh>>& inout,
745 std::vector<std::string>& /*outinfo*/ ) const {
746 Mesh out_mesh;
747 const double s_start = lane_section.GetDistance();
748 const double s_end = lane_section.GetDistance() + lane_section.GetLength();
749 double s_current = s_start;
750 std::vector<geom::Vector3D> vertices;
751 std::vector<size_t> indices;
752
753 do {
754 //Get Lane info
755 const carla::road::element::RoadInfoMarkRecord* road_info_mark = lane.GetInfo<carla::road::element::RoadInfoMarkRecord>(s_current);
756 if (road_info_mark != nullptr) {
757 carla::road::element::LaneMarking lane_mark_info(*road_info_mark);
758
759 switch (lane_mark_info.type) {
760 case carla::road::element::LaneMarking::Type::Solid: {
761 size_t currentIndex = out_mesh.GetVertices().size() + 1;
762
763 std::pair<geom::Vector3D, geom::Vector3D> edges =
764 ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width, 0.0f);
765
766 out_mesh.AddVertex(edges.first);
767 out_mesh.AddVertex(edges.second);
768
769 // Close this step's quad against a rung one resolution-step
770 // ahead instead of leaning on the *next* do-while iteration to
771 // supply it (as a bare "add 2 vertices, index 4" would): if the
772 // mark type changes to one this switch treats as a no-op (None,
773 // SolidSolid, SolidBroken, ...), that iteration adds no
774 // vertices at all, so these dangling indices used to only get
775 // resolved by the post-loop closing block below -- using
776 // s_end/the overshot s_current -- which stretched this rung
777 // into one long, warped quad spanning however much unmarked
778 // road followed. Self-close every Solid step, same as Broken
779 // already does.
780 s_current += road_param.resolution;
781 if (s_current > s_end) {
782 s_current = s_end;
783 }
784
785 edges = ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width, 0.0f);
786
787 out_mesh.AddVertex(edges.first);
788 out_mesh.AddVertex(edges.second);
789
790 out_mesh.AddIndex(currentIndex);
791 out_mesh.AddIndex(currentIndex + 1);
792 out_mesh.AddIndex(currentIndex + 2);
793
794 out_mesh.AddIndex(currentIndex + 1);
795 out_mesh.AddIndex(currentIndex + 3);
796 out_mesh.AddIndex(currentIndex + 2);
797
798 break;
799 }
800 case carla::road::element::LaneMarking::Type::Broken: {
801 size_t currentIndex = out_mesh.GetVertices().size() + 1;
802
803 std::pair<geom::Vector3D, geom::Vector3D> edges =
804 ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width, road_param.extra_lane_width);
805
806 out_mesh.AddVertex(edges.first);
807 out_mesh.AddVertex(edges.second);
808
809 s_current += road_param.resolution * 3;
810 if (s_current > s_end)
811 {
812 s_current = s_end;
813 }
814
815 edges = ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width, road_param.extra_lane_width);
816
817 out_mesh.AddVertex(edges.first);
818 out_mesh.AddVertex(edges.second);
819
820 out_mesh.AddIndex(currentIndex);
821 out_mesh.AddIndex(currentIndex + 1);
822 out_mesh.AddIndex(currentIndex + 2);
823
824 out_mesh.AddIndex(currentIndex + 1);
825 out_mesh.AddIndex(currentIndex + 3);
826 out_mesh.AddIndex(currentIndex + 2);
827
828 s_current += road_param.resolution * 3;
829
830 break;
831 }
832 case carla::road::element::LaneMarking::Type::SolidSolid: {
833 s_current += road_param.resolution;
834 break;
835 }
836 case carla::road::element::LaneMarking::Type::SolidBroken: {
837 s_current += road_param.resolution;
838 break;
839 }
840 case carla::road::element::LaneMarking::Type::BrokenSolid: {
841 s_current += road_param.resolution;
842 break;
843 }
844 case carla::road::element::LaneMarking::Type::BrokenBroken: {
845 s_current += road_param.resolution;
846 break;
847 }
848 case carla::road::element::LaneMarking::Type::BottsDots: {
849 s_current += road_param.resolution;
850 break;
851 }
852 case carla::road::element::LaneMarking::Type::Grass: {
853 s_current += road_param.resolution;
854 break;
855 }
856 case carla::road::element::LaneMarking::Type::Curb: {
857 s_current += road_param.resolution;
858 break;
859 }
860 case carla::road::element::LaneMarking::Type::Other: {
861 s_current += road_param.resolution;
862 break;
863 }
864 default: {
865 s_current += road_param.resolution;
866 break;
867 }
868 }
869 }
870 } while (s_current < s_end);
871
872 if (out_mesh.IsValid()) {
873 const carla::road::element::RoadInfoMarkRecord* road_info_mark = lane.GetInfo<carla::road::element::RoadInfoMarkRecord>(s_current);
874 if (road_info_mark != nullptr) {
875 carla::road::element::LaneMarking lane_mark_info(*road_info_mark);
876
877 std::pair<geom::Vector3D, geom::Vector3D> edges =
878 ComputeEdgesForLanemark(lane_section, lane, s_end, lane_mark_info.width, 0.0f);
879
880 out_mesh.AddVertex(edges.first);
881 out_mesh.AddVertex(edges.second);
882 }
883 inout.push_back(std::make_unique<Mesh>(out_mesh));
884 }
885 }
886
887 void MeshFactory::GenerateLaneMarksForCenterLine(
888 const road::Road& road,
889 const road::LaneSection& lane_section,
890 const road::Lane& lane,
891 std::vector<std::unique_ptr<Mesh>>& inout,
892 std::vector<std::string>& /*outinfo*/ ) const
893 {
894 Mesh out_mesh;
895 const double s_start = lane_section.GetDistance();
896 const double s_end = lane_section.GetDistance() + lane_section.GetLength();
897 double s_current = s_start;
898 std::vector<geom::Vector3D> vertices;
899 std::vector<size_t> indices;
900
901 do {
902 //Get Lane info
903 const carla::road::element::RoadInfoMarkRecord* road_info_mark = lane.GetInfo<carla::road::element::RoadInfoMarkRecord>(s_current);
904 if (road_info_mark != nullptr) {
905 carla::road::element::LaneMarking lane_mark_info(*road_info_mark);
906
907 switch (lane_mark_info.type) {
908 case carla::road::element::LaneMarking::Type::Solid: {
909 size_t currentIndex = out_mesh.GetVertices().size() + 1;
910
911 carla::road::element::DirectedPoint rightpoint = road.GetDirectedPointIn(s_current);
912 carla::road::element::DirectedPoint leftpoint = rightpoint;
913
914 rightpoint.ApplyLateralOffset(static_cast<float>(lane_mark_info.width * 0.5));
915 leftpoint.ApplyLateralOffset(static_cast<float>(lane_mark_info.width * -0.5));
916
917 // Unreal's Y axis hack
918 rightpoint.location.y *= -1;
919 leftpoint.location.y *= -1;
920
921 out_mesh.AddVertex(rightpoint.location);
922 out_mesh.AddVertex(leftpoint.location);
923
924 // Close this step's quad against a rung one resolution-step
925 // ahead instead of leaning on the *next* do-while iteration to
926 // supply it -- see the matching comment on the Solid case in
927 // GenerateLaneMarksForNotCenterLine. Without this, a centerline
928 // that turns into a no-op mark type (None, SolidSolid,
929 // SolidBroken double-yellow, ...) partway down the lane left
930 // this rung's quad dangling until the post-loop closing block
931 // stitched it to s_end/the overshot s_current instead, warping
932 // it into one long quad spanning however much unmarked road
933 // followed.
934 s_current += road_param.resolution;
935 if (s_current > s_end) {
936 s_current = s_end;
937 }
938
939 rightpoint = road.GetDirectedPointIn(s_current);
940 leftpoint = rightpoint;
941
942 rightpoint.ApplyLateralOffset(static_cast<float>(lane_mark_info.width * 0.5));
943 leftpoint.ApplyLateralOffset(static_cast<float>(lane_mark_info.width * -0.5));
944
945 rightpoint.location.y *= -1;
946 leftpoint.location.y *= -1;
947
948 out_mesh.AddVertex(rightpoint.location);
949 out_mesh.AddVertex(leftpoint.location);
950
951 out_mesh.AddIndex(currentIndex);
952 out_mesh.AddIndex(currentIndex + 1);
953 out_mesh.AddIndex(currentIndex + 2);
954
955 out_mesh.AddIndex(currentIndex + 1);
956 out_mesh.AddIndex(currentIndex + 3);
957 out_mesh.AddIndex(currentIndex + 2);
958
959 break;
960 }
961 case carla::road::element::LaneMarking::Type::Broken: {
962 size_t currentIndex = out_mesh.GetVertices().size() + 1;
963
964 std::pair<geom::Vector3D, geom::Vector3D> edges =
965 ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width, road_param.extra_lane_width);
966
967 out_mesh.AddVertex(edges.first);
968 out_mesh.AddVertex(edges.second);
969
970 s_current += road_param.resolution * 3;
971 if (s_current > s_end) {
972 s_current = s_end;
973 }
974
975 edges = ComputeEdgesForLanemark(lane_section, lane, s_current, lane_mark_info.width, road_param.extra_lane_width);
976
977 out_mesh.AddVertex(edges.first);
978 out_mesh.AddVertex(edges.second);
979
980 out_mesh.AddIndex(currentIndex);
981 out_mesh.AddIndex(currentIndex + 1);
982 out_mesh.AddIndex(currentIndex + 2);
983
984 out_mesh.AddIndex(currentIndex + 1);
985 out_mesh.AddIndex(currentIndex + 3);
986 out_mesh.AddIndex(currentIndex + 2);
987
988 s_current += road_param.resolution * 3;
989
990 break;
991 }
992 case carla::road::element::LaneMarking::Type::SolidSolid: {
993 s_current += road_param.resolution;
994 break;
995 }
996 case carla::road::element::LaneMarking::Type::SolidBroken: {
997 s_current += road_param.resolution;
998 break;
999 }
1000 case carla::road::element::LaneMarking::Type::BrokenSolid: {
1001 s_current += road_param.resolution;
1002 break;
1003 }
1004 case carla::road::element::LaneMarking::Type::BrokenBroken: {
1005 s_current += road_param.resolution;
1006 break;
1007 }
1008 case carla::road::element::LaneMarking::Type::BottsDots: {
1009 s_current += road_param.resolution;
1010 break;
1011 }
1012 case carla::road::element::LaneMarking::Type::Grass: {
1013 s_current += road_param.resolution;
1014 break;
1015 }
1016 case carla::road::element::LaneMarking::Type::Curb: {
1017 s_current += road_param.resolution;
1018 break;
1019 }
1020 case carla::road::element::LaneMarking::Type::Other: {
1021 s_current += road_param.resolution;
1022 break;
1023 }
1024 default: {
1025 s_current += road_param.resolution;
1026 break;
1027 }
1028 }
1029 }
1030 } while (s_current < s_end);
1031
1032 if (out_mesh.IsValid()) {
1033 const carla::road::element::RoadInfoMarkRecord* road_info_mark = lane.GetInfo<carla::road::element::RoadInfoMarkRecord>(s_current);
1034 if (road_info_mark != nullptr)
1035 {
1036 carla::road::element::LaneMarking lane_mark_info(*road_info_mark);
1037 carla::road::element::DirectedPoint rightpoint = road.GetDirectedPointIn(s_current);
1038 carla::road::element::DirectedPoint leftpoint = rightpoint;
1039
1040 rightpoint.ApplyLateralOffset(static_cast<float>(lane_mark_info.width * 0.5));
1041 leftpoint.ApplyLateralOffset(static_cast<float>(lane_mark_info.width * -0.5));
1042
1043 // Unreal's Y axis hack
1044 rightpoint.location.y *= -1;
1045 leftpoint.location.y *= -1;
1046
1047 out_mesh.AddVertex(rightpoint.location);
1048 out_mesh.AddVertex(leftpoint.location);
1049
1050 }
1051 inout.push_back(std::make_unique<Mesh>(out_mesh));
1052 }
1053 }
1054
1059 struct VertexNeighbors {
1060 Mesh::vertex_type* vertex;
1061 std::vector<VertexWeight> neighbors;
1062 };
1068
1069 // Helper function to compute the weight of neighboring vertices
1070 static VertexWeight ComputeVertexWeight(
1071 const MeshFactory::RoadParameters &road_param,
1072 const VertexInfo &vertex_info,
1073 const VertexInfo &neighbor_info) {
1074 const float distance3D = geom::Math::Distance(*vertex_info.vertex, *neighbor_info.vertex);
1075 // Ignore vertices beyond a certain distance
1076 if(distance3D > road_param.max_weight_distance) {
1077 return {neighbor_info.vertex, 0};
1078 }
1079 if(abs(distance3D) < EPSILON) {
1080 return {neighbor_info.vertex, 0};
1081 }
1082 float weight = geom::Math::Clamp<float>(1.0f / distance3D, 0.0f, 100000.0f);
1083
1084 // Additional weight to vertices in the same lane
1085 if(vertex_info.lane_mesh_idx == neighbor_info.lane_mesh_idx) {
1086 weight *= road_param.same_lane_weight_multiplier;
1087 // Further additional weight for fixed verices
1088 if(neighbor_info.is_static) {
1089 weight *= road_param.lane_ends_multiplier;
1090 }
1091 }
1092 return {neighbor_info.vertex, weight};
1093 }
1094
1095 // Helper function to compute neighborhoord of vertices and their weights
1096 std::vector<VertexNeighbors> GetVertexNeighborhoodAndWeights(
1097 const MeshFactory::RoadParameters &road_param,
1098 std::vector<std::unique_ptr<Mesh>> &lane_meshes) {
1099 // Build rtree for neighborhood queries
1100 using Rtree = geom::PointCloudRtree<VertexInfo>;
1101 using Point = Rtree::BPoint;
1102 Rtree rtree;
1103 for (size_t lane_mesh_idx = 0; lane_mesh_idx < lane_meshes.size(); ++lane_mesh_idx) {
1104 auto& mesh = lane_meshes[lane_mesh_idx];
1105 for(size_t i = 0; i < mesh->GetVerticesNum(); ++i) {
1106 auto& vertex = mesh->GetVertices()[i];
1107 Point point(vertex.x, vertex.y, vertex.z);
1108 if (i < 2 || i >= mesh->GetVerticesNum() - 2) {
1109 rtree.InsertElement({point, {&vertex, lane_mesh_idx, true}});
1110 } else {
1111 rtree.InsertElement({point, {&vertex, lane_mesh_idx, false}});
1112 }
1113 }
1114 }
1115
1116 // Find neighbors for each vertex and compute their weight
1117 std::vector<VertexNeighbors> vertices_neighborhoods;
1118 for (size_t lane_mesh_idx = 0; lane_mesh_idx < lane_meshes.size(); ++lane_mesh_idx) {
1119 auto& mesh = lane_meshes[lane_mesh_idx];
1120 for(size_t i = 0; i < mesh->GetVerticesNum(); ++i) {
1121 if (i > 2 && i < mesh->GetVerticesNum() - 2) {
1122 auto& vertex = mesh->GetVertices()[i];
1123 Point point(vertex.x, vertex.y, vertex.z);
1124 auto closest_vertices = rtree.GetNearestNeighbours(point, 20);
1125 VertexNeighbors vertex_neighborhood;
1126 vertex_neighborhood.vertex = &vertex;
1127 for(auto& close_vertex : closest_vertices) {
1128 auto &vertex_info = close_vertex.second;
1129 if(&vertex == vertex_info.vertex) {
1130 continue;
1131 }
1132 auto vertex_weight = ComputeVertexWeight(
1133 road_param, {&vertex, lane_mesh_idx, false}, vertex_info);
1134 if(vertex_weight.weight > 0)
1135 vertex_neighborhood.neighbors.push_back(vertex_weight);
1136 }
1137 vertices_neighborhoods.push_back(vertex_neighborhood);
1138 }
1139 }
1140 }
1141 return vertices_neighborhoods;
1142 }
1143
1144 std::unique_ptr<Mesh> MeshFactory::MergeAndSmooth(std::vector<std::unique_ptr<Mesh>> &lane_meshes) const {
1145 geom::Mesh out_mesh;
1146
1147 auto vertices_neighborhoods = GetVertexNeighborhoodAndWeights(road_param, lane_meshes);
1148
1149 // Laplacian function
1150 auto Laplacian = [&](const Mesh::vertex_type* vertex, const std::vector<VertexWeight> &neighbors) -> double {
1151 double sum = 0;
1152 double sum_weight = 0;
1153 for(auto &element : neighbors) {
1154 sum += (element.vertex->z - vertex->z)*element.weight;
1155 sum_weight += element.weight;
1156 }
1157 if(sum_weight > 0)
1158 return sum / sum_weight;
1159 else
1160 return 0;
1161 };
1162 // Run iterative algorithm
1163 double lambda = 0.5;
1164 int iterations = 100;
1165 for(int iter = 0; iter < iterations; ++iter) {
1166 for (auto& vertex_neighborhood : vertices_neighborhoods) {
1167 auto * vertex = vertex_neighborhood.vertex;
1168 vertex->z += static_cast<float>(lambda*Laplacian(vertex, vertex_neighborhood.neighbors));
1169 }
1170 }
1171
1172 for(auto &mesh : lane_meshes) {
1173 out_mesh += *mesh;
1174 }
1175
1176 return std::make_unique<Mesh>(out_mesh);
1177 }
1178
1179
1180 std::pair<geom::Vector3D, geom::Vector3D> MeshFactory::ComputeEdgesForLanemark(
1181 const road::LaneSection& lane_section,
1182 const road::Lane& lane,
1183 const double s_current,
1184 const double lanemark_width,
1185 const float extra_width) const {
1186 std::pair<geom::Vector3D, geom::Vector3D> edges =
1187 lane.GetCornerPositions(s_current, extra_width);
1188
1189 geom::Vector3D director;
1190 if (edges.first != edges.second) {
1191 // `!=` is an exact floating-point compare, so two corners that are
1192 // technically distinct but only a few nanometers apart still take
1193 // this branch; the old `director /= director.Length()` then divides
1194 // by a near-zero length and blows the vector up to a huge magnitude.
1195 // MakeUnitVector() guards that: below its epsilon it returns the
1196 // (still tiny) input unchanged instead of exploding, so a degenerate
1197 // cross-section just yields a near-zero-length marking segment
1198 // instead of a vertex millions of meters away (which was corrupting
1199 // the OBJ handed to RecastBuilder and blowing up its voxel-grid
1200 // sizing -- see the RecastBuilder-hang investigation).
1201 director = (edges.second - edges.first).MakeUnitVector();
1202 } else {
1203 const std::map<road::LaneId, road::Lane> & lanes = lane_section.GetLanes();
1204 for (const auto& lane_pair : lanes) {
1205 std::pair<geom::Vector3D, geom::Vector3D> another_edge =
1206 lane_pair.second.GetCornerPositions(s_current, extra_width);
1207 if (another_edge.first != another_edge.second) {
1208 director = (another_edge.second - another_edge.first).MakeUnitVector();
1209 break;
1210 }
1211 }
1212 }
1213 geom::Vector3D endmarking = edges.first + director * static_cast<float>(lanemark_width);
1214 return std::make_pair(edges.first, endmarking);
1215 }
1216
1217} // namespace geom
1218} // namespace carla
DEBUG_ASSERT
#define DEBUG_ASSERT(predicate)
Definition Debug.h:66
RELEASE_ASSERT
#define RELEASE_ASSERT(pred)
Definition Debug.h:84
point
geom::Point point
Definition LaneGraph.cpp:41
road
std::map< std::string, unsigned int > road
Definition XodrExport.cpp:30
carla::geom::Math::Distance
static auto Distance(const Vector3D &a, const Vector3D &b)
Definition Math.h:78
carla::geom::MeshFactory::GenerateRightWall
std::unique_ptr< Mesh > GenerateRightWall(const road::Lane &lane, const double s_start, const double s_end) const
Generates a wall-like mesh at the right side of the lane.
Definition MeshFactory.cpp:379
carla::geom::MeshFactory::GenerateSidewalk
std::unique_ptr< Mesh > GenerateSidewalk(const road::LaneSection &lane_section) const
Definition MeshFactory.cpp:232
carla::geom::MeshFactory::GenerateLeftWall
std::unique_ptr< Mesh > GenerateLeftWall(const road::Lane &lane, const double s_start, const double s_end) const
Generates a wall-like mesh at the left side of the lane.
Definition MeshFactory.cpp:430
carla::geom::MeshFactory::MergeAndSmooth
std::unique_ptr< Mesh > MergeAndSmooth(std::vector< std::unique_ptr< Mesh > > &lane_meshes) const
Definition MeshFactory.cpp:1144
carla::geom::MeshFactory::GenerateLaneMarksForCenterLine
void GenerateLaneMarksForCenterLine(const road::Road &road, const road::LaneSection &lane_section, const road::Lane &lane, std::vector< std::unique_ptr< Mesh > > &inout, std::vector< std::string > &outinfo) const
Definition MeshFactory.cpp:887
carla::geom::MeshFactory::ComputeEdgesForLanemark
std::pair< geom::Vector3D, geom::Vector3D > ComputeEdgesForLanemark(const road::LaneSection &lane_section, const road::Lane &lane, const double s_current, const double lanemark_width, const float extra_width) const
Definition MeshFactory.cpp:1180
carla::geom::MeshFactory::GenerateWallsWithMaxLen
std::vector< std::unique_ptr< Mesh > > GenerateWallsWithMaxLen(const road::Road &road) const
Generates a list of meshes that defines a road safety wall with a maximum length.
Definition MeshFactory.cpp:615
carla::geom::MeshFactory::GenerateLaneMarkForRoad
void GenerateLaneMarkForRoad(const road::Road &road, std::vector< std::unique_ptr< Mesh > > &inout, std::vector< std::string > &outinfo) const
Definition MeshFactory.cpp:719
carla::geom::MeshFactory::MeshFactory
MeshFactory(rpc::OpendriveGenerationParameters params=rpc::OpendriveGenerationParameters())
Definition MeshFactory.cpp:21
carla::geom::MeshFactory::road_param
RoadParameters road_param
Definition MeshFactory.h:149
carla::geom::MeshFactory::Generate
std::unique_ptr< Mesh > Generate(const road::Road &road) const
Generates a mesh that defines a road.
Definition MeshFactory.cpp:34
carla::geom::MeshFactory::GenerateLaneMarksForNotCenterLine
void GenerateLaneMarksForNotCenterLine(const road::LaneSection &lane_section, const road::Lane &lane, std::vector< std::unique_ptr< Mesh > > &inout, std::vector< std::string > &outinfo) const
Definition MeshFactory.cpp:741
carla::geom::MeshFactory::GenerateLaneSectionOrdered
void GenerateLaneSectionOrdered(const road::LaneSection &lane_section, std::map< carla::road::Lane::LaneType, std::vector< std::unique_ptr< Mesh > > > &result) const
Generates a mesh that defines a lane section.
Definition MeshFactory.cpp:194
carla::geom::MeshFactory::GenerateTesselated
std::unique_ptr< Mesh > GenerateTesselated(const road::Lane &lane, const double s_start, const double s_end) const
Generates a mesh that defines a lane from a given s start and end with bigger tesselation.
Definition MeshFactory.cpp:112
carla::geom::MeshFactory::GenerateAllWithMaxLen
std::vector< std::unique_ptr< Mesh > > GenerateAllWithMaxLen(const road::Road &road) const
Generates a chunked road with all the features needed for simulation.
Definition MeshFactory.cpp:674
carla::geom::MeshFactory::GenerateWithMaxLen
std::vector< std::unique_ptr< Mesh > > GenerateWithMaxLen(const road::Road &road) const
Generates a list of meshes that defines a road with a maximum length.
Definition MeshFactory.cpp:481
carla::geom::MeshFactory::GenerateOrderedWithMaxLen
std::map< carla::road::Lane::LaneType, std::vector< std::unique_ptr< Mesh > > > GenerateOrderedWithMaxLen(const road::Road &road) const
Generates a list of meshes that defines a road with a maximum length.
Definition MeshFactory.cpp:522
carla::geom::MeshFactory::GenerateWalls
std::unique_ptr< Mesh > GenerateWalls(const road::LaneSection &lane_section) const
Genrates a mesh representing a wall on the road corners to avoid cars falling down.
Definition MeshFactory.cpp:357
carla::geom::MeshFactory::GenerateAllOrderedWithMaxLen
void GenerateAllOrderedWithMaxLen(const road::Road &road, std::map< road::Lane::LaneType, std::vector< std::unique_ptr< Mesh > > > &roads) const
Definition MeshFactory.cpp:704
carla::geom::Mesh
Mesh data container, validator and exporter.
Definition Mesh.h:44
carla::geom::Mesh::AddIndex
void AddIndex(index_type index)
Appends a index to the indexes list.
Definition Mesh.cpp:101
carla::geom::Mesh::AddVertex
void AddVertex(vertex_type vertex)
Appends a vertex to the vertices list.
Definition Mesh.cpp:89
carla::geom::Mesh::AddVertices
void AddVertices(const std::vector< vertex_type > &vertices)
Appends a vertex to the vertices list.
Definition Mesh.cpp:93
carla::geom::Mesh::IsValid
bool IsValid() const
Check if the mesh can be valid or not.
Definition Mesh.cpp:20
carla::geom::Mesh::AddUVs
void AddUVs(const std::vector< uv_type > &uv)
Appends uvs.
Definition Mesh.cpp:109
carla::geom::Mesh::AddMaterial
void AddMaterial(const std::string &material_name)
Starts applying a new material to the new added triangles.
Definition Mesh.cpp:113
carla::geom::Mesh::EndMaterial
void EndMaterial()
Stops applying the material to the new added triangles.
Definition Mesh.cpp:129
carla::geom::Mesh::AddTriangleStrip
void AddTriangleStrip(const std::vector< vertex_type > &vertices)
Adds a triangle strip to the mesh, vertex order is counterclockwise.
Definition Mesh.cpp:47
carla::geom::Mesh::GetVertices
const std::vector< vertex_type > & GetVertices() const
Definition Mesh.cpp:307
carla::geom::PointCloudRtree
Rtree class working with 3D point clouds.
Definition Rtree.h:30
carla::road::LaneSection::GetLanes
std::map< LaneId, Lane > & GetLanes()
Definition road/LaneSection.cpp:47
carla::road::Lane::IsStraight
bool IsStraight() const
Checks whether the geometry is straight or not.
Definition road/Lane.cpp:67
carla::road::Lane::GetType
LaneType GetType() const
Definition road/Lane.cpp:38
carla::road::Lane::GetInfo
const T * GetInfo(const double s) const
Definition road/Lane.h:79
carla::road::Lane::LaneType
LaneType
Can be used as flags.
Definition road/Lane.h:29
carla::road::Lane::GetDistance
double GetDistance() const
Definition road/Lane.cpp:46
carla::road::Lane::GetCornerPositions
std::pair< geom::Vector3D, geom::Vector3D > GetCornerPositions(const double s, const float extra_width=0.f) const
Computes the location of the edges given a s.
Definition road/Lane.cpp:216
carla::road::Lane::GetId
LaneId GetId() const
Definition road/Lane.cpp:34
carla::road::Lane::GetLength
double GetLength() const
Definition road/Lane.cpp:51
carla::road::element::RoadInfoMarkRecord
Each lane within a road cross section can be provided with several road markentries.
Definition RoadInfoMarkRecord.h:25
carla::geom::EPSILON
static constexpr double EPSILON
We use this epsilon to shift the waypoints away from the edges of the lane sections to avoid floating...
Definition MeshFactory.cpp:31
carla::geom::ComputeVertexWeight
static VertexWeight ComputeVertexWeight(const MeshFactory::RoadParameters &road_param, const VertexInfo &vertex_info, const VertexInfo &neighbor_info)
Definition MeshFactory.cpp:1070
carla::geom::MESH_EPSILON
static constexpr double MESH_EPSILON
Definition MeshFactory.cpp:32
carla::geom::GetVertexNeighborhoodAndWeights
std::vector< VertexNeighbors > GetVertexNeighborhoodAndWeights(const MeshFactory::RoadParameters &road_param, std::vector< std::unique_ptr< Mesh > > &lane_meshes)
Definition MeshFactory.cpp:1096
carla
This file contains definitions of common data structures used in traffic manager.
Definition SignComponent.h:21
carla::geom::MeshFactory::RoadParameters
Parameters for the road generation.
Definition MeshFactory.h:137
carla::geom::VertexInfo::vertex
Mesh::vertex_type * vertex
Definition MeshFactory.cpp:1064
carla::geom::VertexNeighbors::neighbors
std::vector< VertexWeight > neighbors
Definition MeshFactory.cpp:1061
carla::geom::VertexWeight::vertex
Mesh::vertex_type * vertex
Definition MeshFactory.cpp:1056
carla::rpc::OpendriveGenerationParameters
Seting for map generation from opendrive without additional geometry.
Definition OpendriveGenerationParameters.h:15
Generated by doxygen 1.9.8