slapin/streaming_world
1
0
Fork 0
Code Issues 32 Pull Requests Packages Projects 2 Releases Wiki Activity

Road lines seems to be working and converted to road nodes

Browse Source
This commit is contained in:
Sergey Lapin
2024-05-20 17:23:23 +03:00
parent 33dd2be5ac
commit d0db1e45a3
2 changed files with 97 additions and 242 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
project.godot
View File

339
src/modules/stream/stream.cpp
View File

@@ -279,9 +279,6 @@ struct RoadLinesProcessing {
int lanes;
int flags;
};
std::vector<Vector3> road_lines_nodes;
std::unordered_map<uint32_t, std::vector<Vector3> >
road_lines_nodes_hash;
HashMap<String, struct road_line> road_lines;
std::vector<Vector3> 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<int> &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<uint32_t, std::vector<Vector3> >
&road_lines_nodes_hash,
std::vector<Vector3> &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<uint32_t, std::vector<Vector3> >
&road_lines_nodes_hash,
std::vector<Vector3> &road_lines_nodes)
{
int i;
List<String> keys;
road_lines.get_key_list(&keys);
List<String>::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<Vector3> &road_lines_nodes)
{
int i, j;
int i;
List<String> keys;
road_lines.get_key_list(&keys);
List<String>::Element *e = keys.front();
while (e) {
std::vector<int> 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<int> 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<int> 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<String> keys;
road_lines.get_key_list(&keys);
List<String>::Element *e = keys.front();
while (e) {
std::vector<int> segments;
String rkey = e->get();
if (road_lines[rkey].indices.size() < 2) {
e = e->next();
continue;
}
create_segments(rkey, segments);
List<String>::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<Vector3> 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<Vector3> &road_lines_nodes)
{
List<String> 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<uint32_t, std::tuple<String, String> > kcmp;
{
@@ -859,7 +655,7 @@ struct RoadLinesProcessing {
*/
}
void create_nodes()
void create_nodes(const std::vector<Vector3> &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<Vector3> &road_lines_nodes)
{
int i;
List<String> keys;
road_lines.get_key_list(&keys);
List<String>::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<Vector3> road_lines_nodes;
std::unordered_map<uint32_t, std::vector<Vector3> >
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<int, RoadLinesProcessing::edgedata>::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);
}