From d0db1e45a333c64c7846eddaa8c2aa5d4570469b Mon Sep 17 00:00:00 2001 From: Sergey Lapin Date: Mon, 20 May 2024 17:23:23 +0300 Subject: [PATCH] Road lines seems to be working and converted to road nodes --- project.godot | 0 src/modules/stream/stream.cpp | 339 ++++++++++------------------------ 2 files changed, 97 insertions(+), 242 deletions(-) delete mode 100644 project.godot diff --git a/project.godot b/project.godot deleted file mode 100644 index e69de29..0000000 diff --git a/src/modules/stream/stream.cpp b/src/modules/stream/stream.cpp index 3ba53c9..4c794bc 100644 --- a/src/modules/stream/stream.cpp +++ b/src/modules/stream/stream.cpp @@ -279,9 +279,6 @@ struct RoadLinesProcessing { int lanes; int flags; }; - std::vector road_lines_nodes; - std::unordered_map > - road_lines_nodes_hash; HashMap road_lines; std::vector nodes; struct edgedata { @@ -307,7 +304,6 @@ struct RoadLinesProcessing { int z = (int)(v.z / 100); return x ^ (y * 100) ^ (z * 10000); } - void create_segments(const String &road, std::vector &segments) { int i; @@ -320,7 +316,11 @@ struct RoadLinesProcessing { } } - void road_lines_curve_index(struct road_line &rline) + void road_lines_curve_index( + struct road_line &rline, + std::unordered_map > + &road_lines_nodes_hash, + std::vector &road_lines_nodes) { int i, j; rline.indices.clear(); @@ -359,44 +359,29 @@ struct RoadLinesProcessing { int index = it - road_lines_nodes.begin(); rline.indices.push_back(index); } - /* - var l = crv.points - crv.indices = [] - for t in l: - var pt = str2var(t).origin - var pt_hash = road_lines_hash(pt) - if road_lines_nodes_hash.has(pt_hash): - var ok = true - for xpt in road_lines_nodes_hash[pt_hash]: - if xpt.distance_squared_to(pt) < 160.0: - ok = false - pt = xpt - break - if ok: - road_lines_nodes_hash[pt_hash].push_back(pt) - road_lines_nodes.push_back(pt) - else: - road_lines_nodes.push_back(pt) - road_lines_nodes_hash[pt_hash] = [pt] - var index = road_lines_nodes.find(pt) - assert(index >= 0) - crv.indices.push_back(index) -# print(crv) - pass - */ } - void index_lines() + void index_lines(std::unordered_map > + &road_lines_nodes_hash, + std::vector &road_lines_nodes) { - int i; List keys; road_lines.get_key_list(&keys); List::Element *e = keys.front(); while (e) { String rkey = e->get(); struct road_line &pt = road_lines[rkey]; - road_lines_curve_index(pt); + pt.indices.clear(); e = e->next(); } + e = keys.front(); + while (e) { + String rkey = e->get(); + struct road_line &pt = road_lines[rkey]; + road_lines_curve_index(pt, road_lines_nodes_hash, + road_lines_nodes); + e = e->next(); + } +#if 0 /* deduplicate */ e = keys.front(); while (e) { @@ -414,38 +399,29 @@ struct RoadLinesProcessing { pt.indices = index; e = e->next(); } +#endif } /* add close points on each line to the line */ - void insert_close_points() + void insert_close_points(std::vector &road_lines_nodes) { - int i, j; + int i; List keys; road_lines.get_key_list(&keys); List::Element *e = keys.front(); - while (e) { - std::vector segments; - String rkey = e->get(); - print_line("line: " + rkey + " " + - itos(road_lines[rkey].indices.size())); - create_segments(rkey, segments); - print_line("segments: " + itos(segments.size())); - int index_count = road_lines[rkey].indices.size(); - int segment_size = segments.size(); - std::vector indices(road_lines[rkey].indices); - for (i = 0; i < index_count; i++) { - int idx3 = indices[i]; - print_line("index: " + itos(i)); - for (j = 0; j < segment_size; j += 3) { + for (i = 0; i < (int)road_lines_nodes.size(); i++) { + int idx3 = i; + while (e) { + int j; + std::vector segments; + String rkey = e->get(); + create_segments(rkey, segments); + for (j = 0; j < (int)segments.size(); j += 3) { int idx1 = segments[j]; int idx2 = segments[j + 1]; int idx = segments[j + 2]; + /* Skip segment point */ if (idx3 == idx1 || idx3 == idx2) - /* - # if point is one of segment ends it is already there - # so no need to insert anything - */ continue; - print_line("segment: " + itos(j / 3)); Vector3 p1 = road_lines_nodes[idx1]; Vector3 p2 = road_lines_nodes[idx2]; Vector3 p3 = road_lines_nodes[idx3]; @@ -464,194 +440,14 @@ struct RoadLinesProcessing { idx3); } } - } - print_line("line: " + rkey + " processed"); - e = e->next(); - } - } - - void insert_close_points2() - { - int i, j; - List keys; - road_lines.get_key_list(&keys); - List::Element *e = keys.front(); - while (e) { - std::vector segments; - String rkey = e->get(); - if (road_lines[rkey].indices.size() < 2) { e = e->next(); - continue; } - create_segments(rkey, segments); - List::Element *r = keys.front(); - while (r) { - String r2key = r->get(); - if (e == r) { - r = r->next(); - continue; - } - if (road_lines[r2key].indices.size() < 2) { - r = r->next(); - continue; - } - /* # insert intersections for points close to road. */ - for (i = 0; - i < (int)road_lines[r2key].indices.size(); - i++) { - int idx3 = road_lines[r2key].indices[i]; - for (j = 0; j < (int)segments.size(); - j += 3) { - int idx1 = segments[j]; - int idx2 = segments[j + 1]; - int idx = segments[j + 2]; - if (idx3 == idx1 || - idx3 == idx2) - /* - # if point is one of segment ends it is already there - # so no need to insert anything - */ - continue; - Vector3 p1 = - road_lines_nodes[idx1]; - Vector3 p2 = - road_lines_nodes[idx2]; - Vector3 p3 = - road_lines_nodes[idx3]; - std::vector seg = { - p1, p2 - }; - Vector3 closest = Geometry:: - get_closest_point_to_segment( - p3, seg.data()); - if (p3.distance_squared_to( - closest) < 160) { - road_lines_nodes[idx3] = - closest; - road_lines[rkey].indices.insert( - road_lines[rkey].indices - .begin() + - idx, - idx3); - } - } - } - r = r->next(); - } - e = e->next(); } } - void update_road_lines_nodes() + void update_road_lines_nodes(std::vector &road_lines_nodes) { List keys; - road_lines.get_key_list(&keys); - road_lines_nodes.clear(); - road_lines_nodes_hash.clear(); - index_lines(); - /* - for k in road_lines.keys(): - var segments = [] - for i1 in range(road_lines[k].indices.size() - 1): - var idx1 = road_lines[k].indices[i1] - var idx2 = road_lines[k].indices[i1 + 1] - var p1 = road_lines_nodes[idx1] - var p2 = road_lines_nodes[idx2] - # keeping index position for vertex insert - segments.push_back([idx1, idx2, i1 + 1]) - for i3 in range(road_lines[k].indices.size()): - var idx3 = road_lines[k].indices[i3] - for seg in segments: - var idx1 = seg[0] - var idx2 = seg[1] - var idx = seg[2] - if idx3 == idx1 || idx3 == idx2: - # if point is one of segment ends it is already there - # so no need to insert anything - continue - var p1 = road_lines_nodes[idx1] - var p2 = road_lines_nodes[idx2] - var p3: Vector3 = road_lines_nodes[idx3] - var closest = Geometry.get_closest_point_to_segment(p3, p1, p2) - if p3.distance_squared_to(closest) < 160: - # we found a point close to the segment - # insert vertex index for intersection point - # change point position to one on segment - road_lines_nodes[idx3] = closest - road_lines[k].indices.insert(idx, idx3) - */ - insert_close_points(); - /* -# find points close to road segments - for k in road_lines.keys(): - if road_lines[k].indices.size() < 2: - continue - var segments = [] - for i1 in range(road_lines[k].indices.size() - 1): - var idx1 = road_lines[k].indices[i1] - var idx2 = road_lines[k].indices[i1 + 1] - var p1 = road_lines_nodes[idx1] - var p2 = road_lines_nodes[idx2] - # keeping index position for vertex insert - segments.push_back([idx1, idx2, i1 + 1]) - for r in road_lines.keys(): - if r == k: - continue - if road_lines[r].indices.size() < 2: - continue - # insert intersections for points close to road. - - for i3 in range(road_lines[r].indices.size()): - var idx3 = road_lines[r].indices[i3] - for seg in segments: - var idx1 = seg[0] - var idx2 = seg[1] - var idx = seg[2] - if idx3 == idx1 || idx3 == idx2: - # if point is one of segment ends it is already there - # so no need to insert anything - continue - var p1 = road_lines_nodes[idx1] - var p2 = road_lines_nodes[idx2] - var p3: Vector3 = road_lines_nodes[idx3] - var closest = Geometry.get_closest_point_to_segment(p3, p1, p2) - if p3.distance_squared_to(closest) < 160: - # we found a point close to the segment - # insert vertex index for intersection point - # change point position to one on segment - road_lines_nodes[idx3] = closest - road_lines[k].indices.insert(idx, idx3) - */ - insert_close_points2(); - /* - - -# # deduplicate indices -# for k in road_lines.keys(): -# var _data = [] -# for i1 in range(road_lines[k].indices.size() - 1): -# var idx1 = road_lines[k].indices[i1] -# if i1 == 0: -# _data.push_back(idx1) -# var i2 = i1 + 1 -# var idx2 = road_lines[k].indices[i2] -# if idx1 != idx2: -# _data.push_back(idx2) -# print(road_lines[k].indices) -# print(_data) -# road_lines[k].indices = _data -# print(road_lines) -# -##intersections between segments -##start - var compare = {} - for k in road_lines.keys(): - for r in road_lines.keys(): - var key = k + "-" + r - var key2 = r + "-" + k - if !compare.has(key) && !compare.has(key2): - compare[key] = [k, r] - */ road_lines.get_key_list(&keys); std::unordered_map > kcmp; { @@ -859,7 +655,7 @@ struct RoadLinesProcessing { */ } - void create_nodes() + void create_nodes(const std::vector &road_lines_nodes) { nodes.resize(road_lines_nodes.size()); memcpy(nodes.data(), road_lines_nodes.data(), @@ -909,15 +705,47 @@ struct RoadLinesProcessing { void create_road_from_lines() { } + void dump_lines(const std::vector &road_lines_nodes) + { + int i; + List keys; + road_lines.get_key_list(&keys); + List::Element *e = keys.front(); + while (e) { + String rkey = e->get(); + struct road_line &pt = road_lines[rkey]; + String outline = rkey + ": "; + for (i = 0; i < (int)pt.indices.size(); i++) { + outline += " " + itos(pt.indices[i]); + } + for (i = 0; i < (int)pt.indices.size(); i++) { + outline += + " " + (road_lines_nodes[pt.indices[i]] + .operator String()); + } + print_line(outline); + e = e->next(); + } + } void road_setup() { - update_road_lines_nodes(); - create_nodes(); + std::vector road_lines_nodes; + std::unordered_map > + road_lines_nodes_hash; + print_line("ROAD SETUP"); + road_lines_nodes.clear(); + road_lines_nodes_hash.clear(); + index_lines(road_lines_nodes_hash, road_lines_nodes); + insert_close_points(road_lines_nodes); + update_road_lines_nodes(road_lines_nodes); + dump_lines(road_lines_nodes); + create_nodes(road_lines_nodes); create_edges(); print_line("NODES:" + itos(nodes.size())); print_line("EDGES:" + itos(edges.size())); create_road_from_lines(); + print_line("ROAD SETUP DONE"); } void read_road_lines_json(const String &road_lines_path) { @@ -1083,6 +911,7 @@ StreamWorld::StreamWorld() void RoadDebug::_notification(int which) { + int i; RoadLinesProcessing *r = RoadLinesProcessing::get_singleton(); std::unordered_map::iterator it; switch (which) { @@ -1098,24 +927,30 @@ void RoadDebug::_notification(int which) imm, Color(1.0f, 0.6f, 0.6f, 1.0f)); for (it = r->edges.begin(); it != r->edges.end(); it++) { - int i; int idx1 = it->first; if (it == r->edges.begin()) { aabb.position = r->nodes[idx1]; aabb.size = Vector3(); } else aabb.expand_to(r->nodes[idx1]); - VisualServer::get_singleton()->immediate_vertex( - imm, r->nodes[idx1]); struct RoadLinesProcessing::edgedata data = it->second; for (i = 0; i < (int)data.neighbors.size(); i++) { int idx2 = data.neighbors[i]; aabb.expand_to(r->nodes[idx2]); + Vector3 d = (r->nodes[idx2] - + r->nodes[idx1]) + .normalized() * + 0.5f; VisualServer::get_singleton() ->immediate_vertex( - imm, r->nodes[idx2]); + imm, + r->nodes[idx1] + d); + VisualServer::get_singleton() + ->immediate_vertex( + imm, + r->nodes[idx2] - d); print_line( "draw line: " + itos(idx1) + " " + itos(idx2) + @@ -1126,6 +961,26 @@ void RoadDebug::_notification(int which) .operator String())); } } + VisualServer::get_singleton()->immediate_color( + imm, Color(0.6f, 0.6f, 1.0f, 1.0f)); + for (it = r->edges.begin(); it != r->edges.end(); + it++) { + int idx1 = it->first; + VisualServer::get_singleton()->immediate_vertex( + imm, + r->nodes[idx1] - Vector3(0, 10, 0)); + VisualServer::get_singleton()->immediate_vertex( + imm, + r->nodes[idx1] + Vector3(0, 100, 0)); + } + VisualServer::get_singleton()->immediate_color( + imm, Color(0.6f, 1.0f, 0.6f, 1.0f)); + for (i = 0; i < (int)r->nodes.size(); i++) { + VisualServer::get_singleton()->immediate_vertex( + imm, r->nodes[i] - Vector3(0, 5, 0)); + VisualServer::get_singleton()->immediate_vertex( + imm, r->nodes[i] + Vector3(0, 80, 0)); + } VisualServer::get_singleton()->immediate_end(imm); set_process(false); }