26 static constexpr
double EPSILON = 10.0 * std::numeric_limits<double>::epsilon();
27 static constexpr
double MESH_EPSILON = 50.0 * std::numeric_limits<double>::epsilon();
34 return std::make_unique<Mesh>(out_mesh);
39 for (
auto &&lane_pair : lane_section.
GetLanes()) {
40 out_mesh += *
Generate(lane_pair.second);
42 return std::make_unique<Mesh>(out_mesh);
48 return Generate(lane, s_start, s_end);
52 const road::Lane &lane,
const double s_start,
const double s_end)
const {
60 if (lane.
GetId() == 0) {
61 return std::make_unique<Mesh>(out_mesh);
63 double s_current = s_start;
65 std::vector<geom::Vector3D> vertices;
70 vertices.push_back(edges.first);
71 vertices.push_back(edges.second);
77 vertices.push_back(edges.first);
78 vertices.push_back(edges.second);
82 }
while(s_current < s_end);
89 vertices.push_back(edges.first);
90 vertices.push_back(edges.second);
98 return std::make_unique<Mesh>(out_mesh);
104 const auto min_lane = lane_section.
GetLanes().begin()->first == 0 ?
105 1 : lane_section.
GetLanes().begin()->first;
106 const auto max_lane = lane_section.
GetLanes().rbegin()->first == 0 ?
107 -1 : lane_section.
GetLanes().rbegin()->first;
109 for (
auto &&lane_pair : lane_section.
GetLanes()) {
110 const auto &lane = lane_pair.second;
111 const double s_start = lane.GetDistance() +
EPSILON;
112 const double s_end = lane.GetDistance() + lane.GetLength() -
EPSILON;
113 if (lane.GetId() == max_lane) {
116 if (lane.GetId() == min_lane) {
120 return std::make_unique<Mesh>(out_mesh);
124 const road::Lane &lane,
const double s_start,
const double s_end)
const {
132 if (lane.
GetId() == 0) {
133 return std::make_unique<Mesh>(out_mesh);
135 double s_current = s_start;
138 std::vector<geom::Vector3D> r_vertices;
143 r_vertices.push_back(edges.first + height_vector);
144 r_vertices.push_back(edges.first);
150 r_vertices.push_back(edges.first + height_vector);
151 r_vertices.push_back(edges.first);
155 }
while(s_current < s_end);
162 r_vertices.push_back(edges.first + height_vector);
163 r_vertices.push_back(edges.first);
171 return std::make_unique<Mesh>(out_mesh);
175 const road::Lane &lane,
const double s_start,
const double s_end)
const {
183 if (lane.
GetId() == 0) {
184 return std::make_unique<Mesh>(out_mesh);
186 double s_current = s_start;
189 std::vector<geom::Vector3D> l_vertices;
194 l_vertices.push_back(edges.second);
195 l_vertices.push_back(edges.second + height_vector);
201 l_vertices.push_back(edges.second);
202 l_vertices.push_back(edges.second + height_vector);
206 }
while(s_current < s_end);
213 l_vertices.push_back(edges.second);
214 l_vertices.push_back(edges.second + height_vector);
222 return std::make_unique<Mesh>(out_mesh);
227 std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
230 mesh_uptr_list.insert(
231 mesh_uptr_list.end(),
232 std::make_move_iterator(section_uptr_list.begin()),
233 std::make_move_iterator(section_uptr_list.end()));
235 return mesh_uptr_list;
240 std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
242 mesh_uptr_list.emplace_back(
Generate(lane_section));
248 Mesh lane_section_mesh;
249 for (
auto &&lane_pair : lane_section.
GetLanes()) {
250 lane_section_mesh += *
Generate(lane_pair.second, s_current, s_until);
252 mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
255 if (s_end - s_current > EPSILON) {
256 Mesh lane_section_mesh;
257 for (
auto &&lane_pair : lane_section.
GetLanes()) {
258 lane_section_mesh += *
Generate(lane_pair.second, s_current, s_end);
260 mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
263 return mesh_uptr_list;
268 std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
271 mesh_uptr_list.insert(
272 mesh_uptr_list.end(),
273 std::make_move_iterator(section_uptr_list.begin()),
274 std::make_move_iterator(section_uptr_list.end()));
276 return mesh_uptr_list;
281 std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
283 const auto min_lane = lane_section.
GetLanes().begin()->first == 0 ?
284 1 : lane_section.
GetLanes().begin()->first;
285 const auto max_lane = lane_section.
GetLanes().rbegin()->first == 0 ?
286 -1 : lane_section.
GetLanes().rbegin()->first;
295 Mesh lane_section_mesh;
296 for (
auto &&lane_pair : lane_section.
GetLanes()) {
297 const auto &lane = lane_pair.second;
298 if (lane.GetId() == max_lane) {
301 if (lane.GetId() == min_lane) {
305 mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
308 if (s_end - s_current > EPSILON) {
309 Mesh lane_section_mesh;
310 for (
auto &&lane_pair : lane_section.
GetLanes()) {
311 const auto &lane = lane_pair.second;
312 if (lane.GetId() == max_lane) {
315 if (lane.GetId() == min_lane) {
319 mesh_uptr_list.emplace_back(std::make_unique<Mesh>(lane_section_mesh));
322 return mesh_uptr_list;
327 std::vector<std::unique_ptr<Mesh>> mesh_uptr_list;
331 mesh_uptr_list.insert(
332 mesh_uptr_list.end(),
333 std::make_move_iterator(roads.begin()),
334 std::make_move_iterator(roads.end()));
340 if (roads.size() == walls.size()) {
341 for (
size_t i = 0; i < walls.size(); ++i) {
342 *mesh_uptr_list[i] += *walls[i];
345 mesh_uptr_list.insert(
346 mesh_uptr_list.end(),
347 std::make_move_iterator(walls.begin()),
348 std::make_move_iterator(walls.end()));
352 return mesh_uptr_list;
377 return {neighbor_info.
vertex, 0};
379 if(abs(distance3D) < EPSILON) {
380 return {neighbor_info.
vertex, 0};
382 float weight = geom::Math::Clamp<float>(1.0f / distance3D, 0.0f, 100000.0f);
392 return {neighbor_info.
vertex, weight};
398 std::vector<std::unique_ptr<Mesh>> &lane_meshes) {
401 using Point = Rtree::BPoint;
403 for (
size_t lane_mesh_idx = 0; lane_mesh_idx < lane_meshes.size(); ++lane_mesh_idx) {
404 auto& mesh = lane_meshes[lane_mesh_idx];
405 for(
size_t i = 0; i < mesh->GetVerticesNum(); ++i) {
406 auto& vertex = mesh->GetVertices()[i];
407 Point point(vertex.x, vertex.y, vertex.z);
408 if (i < 2 || i >= mesh->GetVerticesNum() - 2) {
409 rtree.InsertElement({point, {&vertex, lane_mesh_idx,
true}});
411 rtree.InsertElement({point, {&vertex, lane_mesh_idx,
false}});
417 std::vector<VertexNeighbors> vertices_neighborhoods;
418 for (
size_t lane_mesh_idx = 0; lane_mesh_idx < lane_meshes.size(); ++lane_mesh_idx) {
419 auto& mesh = lane_meshes[lane_mesh_idx];
420 for(
size_t i = 0; i < mesh->GetVerticesNum(); ++i) {
421 if (i > 2 && i < mesh->GetVerticesNum() - 2) {
422 auto& vertex = mesh->GetVertices()[i];
423 Point point(vertex.x, vertex.y, vertex.z);
424 auto closest_vertices = rtree.GetNearestNeighbours(point, 20);
426 vertex_neighborhood.
vertex = &vertex;
427 for(
auto& close_vertex : closest_vertices) {
428 auto &vertex_info = close_vertex.second;
429 if(&vertex == vertex_info.vertex) {
433 road_param, {&vertex, lane_mesh_idx,
false}, vertex_info);
434 if(vertex_weight.weight > 0)
435 vertex_neighborhood.
neighbors.push_back(vertex_weight);
437 vertices_neighborhoods.push_back(vertex_neighborhood);
441 return vertices_neighborhoods;
450 auto Laplacian = [&](
const Mesh::vertex_type* vertex,
const std::vector<VertexWeight> &neighbors) ->
double {
452 double sum_weight = 0;
453 for(
auto &element : neighbors) {
454 sum += (element.vertex->z - vertex->
z)*element.weight;
455 sum_weight += element.weight;
458 return sum / sum_weight;
464 int iterations = 100;
465 for(
int iter = 0; iter < iterations; ++iter) {
466 for (
auto& vertex_neighborhood : vertices_neighborhoods) {
467 auto * vertex = vertex_neighborhood.vertex;
468 vertex->
z +=
static_cast<float>(lambda*Laplacian(vertex, vertex_neighborhood.neighbors));
472 for(
auto &mesh : lane_meshes) {
476 return std::make_unique<Mesh>(out_mesh);
static auto Distance(const Vector3D &a, const Vector3D &b)
Seting for map generation from opendrive without additional geometry.
double GetDistance() const
float same_lane_weight_multiplier
static VertexWeight ComputeVertexWeight(const MeshFactory::RoadParameters &road_param, const VertexInfo &vertex_info, const VertexInfo &neighbor_info)
std::unique_ptr< Mesh > Generate(const road::Road &road) const
Generates a mesh that defines a road.
bgi::rtree< SpatialTreeEntry, bgi::rstar< 16 > > Rtree
std::unique_ptr< Mesh > MergeAndSmooth(std::vector< std::unique_ptr< Mesh >> &lane_meshes) const
std::vector< VertexNeighbors > GetVertexNeighborhoodAndWeights(const MeshFactory::RoadParameters &road_param, std::vector< std::unique_ptr< Mesh >> &lane_meshes)
double GetDistance() const
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.
This file contains definitions of common data structures used in traffic manager. ...
Mesh::vertex_type * vertex
void AddMaterial(const std::string &material_name)
Starts applying a new material to the new added triangles.
void EndMaterial()
Stops applying the material to the new added triangles.
#define DEBUG_ASSERT(predicate)
auto GetLaneSections() const
RoadParameters road_param
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.
#define RELEASE_ASSERT(pred)
MeshFactory(rpc::OpendriveGenerationParameters params=rpc::OpendriveGenerationParameters())
std::vector< VertexWeight > neighbors
void AddTriangleStrip(const std::vector< vertex_type > &vertices)
Adds a triangle strip to the mesh, vertex order is counterclockwise.
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.
Mesh data container, validator and exporter.
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.
static constexpr double MESH_EPSILON
std::map< LaneId, Lane > & GetLanes()
bool IsStraight() const
Checks whether the geometry is straight or not.
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. ...
std::vector< std::unique_ptr< Mesh > > GenerateAllWithMaxLen(const road::Road &road) const
Generates a chunked road with all the features needed for simulation.
static constexpr double EPSILON
We use this epsilon to shift the waypoints away from the edges of the lane sections to avoid floating...
Parameters for the road generation.
Mesh::vertex_type * vertex
Rtree class working with 3D point clouds.
Mesh::vertex_type * vertex
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.
float lane_ends_multiplier
float max_weight_distance