Road lines seems to be working and converted to road nodes

src/modules/stream/stream.cpp

@@ -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) {
+		for (i = 0; i < (int)road_lines_nodes.size(); i++) {
-			std::vector<int> segments;
+			int idx3 = i;
-			String rkey = e->get();
+			while (e) {
-			print_line("line: " + rkey + " " +
+				int j;
-				   itos(road_lines[rkey].indices.size()));
+				std::vector<int> segments;
-			create_segments(rkey, segments);
+				String rkey = e->get();
-			print_line("segments: " + itos(segments.size()));
+				create_segments(rkey, segments);
-			int index_count = road_lines[rkey].indices.size();
+				for (j = 0; j < (int)segments.size(); j += 3) {
-			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) {
 					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();
+		std::vector<Vector3> road_lines_nodes;
-		create_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);
 		}