Almost separated the buildings editor

src/modules/stream/road_lines_data.cpp

@@ -8,8 +8,13 @@
 #include <core/os/dir_access.h>
 #include <core/os/time.h>
 #include <core/math/geometry.h>
+#include <scene/3d/immediate_geometry.h>
+#include <scene/main/viewport.h>
 #include "from_string.h"
 #include "road_lines_data.h"
+
+static ImmediateGeometry *debug_im;
+static Ref<Material> debug_material;
 RoadLinesData::RoadLinesData()
 {
 	int i;
@@ -66,6 +71,14 @@ RoadLinesData *RoadLinesData::get_singleton()
 	return singleton;
 }
 
+RoadLinesData::~RoadLinesData()
+{
+	if (debug_im) {
+		memdelete(debug_im);
+		debug_im = nullptr;
+	}
+}
+
 void RoadLinesData::cleanup()
 {
 	memdelete(singleton);
@@ -211,54 +224,72 @@ void RoadLinesData::index_lines(
 		e = e->next();
 	}
 }
+static inline int get_segment_index(const String &road, int pos)
+{
+	RoadLinesData *rld = RoadLinesData::get_singleton();
+	int idx = rld->lines[road].indices[pos];
+	return idx;
+}
 void RoadLinesData::create_segments(const String &road,
 				    std::vector<int> &segments)
 {
 	int i;
 	RoadLinesData *rld = RoadLinesData::get_singleton();
 	for (i = 0; i < (int)rld->lines[road].indices.size() - 1; i++) {
-		int idx1 = rld->lines[road].indices[i];
-		int idx2 = rld->lines[road].indices[i + 1];
-		segments.push_back(idx1);
-		segments.push_back(idx2);
+		segments.push_back(i);
 		segments.push_back(i + 1);
 	}
 }
 /* add close points on each line to the line */
-void RoadLinesData::insert_close_points(std::vector<Vector3> &road_lines_nodes)
+void RoadLinesData::insert_close_points(std::vector<Vector3> &road_lines_nodes,
+					float distance_squared)
 {
 	int i;
 	List<String> keys;
-	RoadLinesData *rld = RoadLinesData::get_singleton();
-	rld->get_road_lines_key_list(&keys);
+	get_road_lines_key_list(&keys);
 	List<String>::Element *e = keys.front();
 	for (i = 0; i < (int)road_lines_nodes.size(); i++) {
 		int idx3 = i;
+		Vector3 p3 = road_lines_nodes[idx3];
+		/* Checking each road point against
+		   all line segments */
 		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];
+			for (j = 0; j < (int)segments.size(); j += 2) {
+				/* indices in road_lines_nodes */
+				int idx1 = get_segment_index(rkey, segments[j]);
+				int idx2 = get_segment_index(rkey,
+							     segments[j + 1]);
+				/* insertion point in line indices
+				   array to split segment and add point */
+				int idx = segments[j + 1];
 				/* Skip segment point */
 				if (idx3 == idx1 || idx3 == idx2)
 					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;
-					rld->lines[rkey].indices.insert(
-						rld->lines[rkey].indices.begin() +
+				/* should be no duplicate points
+				   in road_lines_nodes */
+				if (closest.is_equal_approx(p1))
+					continue;
+				if (closest.is_equal_approx(p2))
+					continue;
+				if (p3.distance_squared_to(closest) <
+				    distance_squared) {
+					/* split segment and replace road
+					   point with a point on segment */
+					lines[rkey].indices.insert(
+						lines[rkey].indices.begin() +
 							idx,
 						idx3);
+					road_lines_nodes[idx3] = closest;
 				}
 			}
 			e = e->next();
@@ -423,13 +454,42 @@ void RoadLinesData::dump_road_lines(const std::vector<Vector3> &road_lines_nodes
 	}
 }
 
+static inline ImmediateGeometry *get_debug_node()
+{
+	debug_im = memnew(ImmediateGeometry);
+	Ref<SpatialMaterial> tmpmat;
+	tmpmat.instance();
+	tmpmat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+	tmpmat->set_flag(SpatialMaterial::FLAG_DISABLE_DEPTH_TEST, true);
+	tmpmat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
+	debug_material = tmpmat;
+	debug_im->set_material_override(debug_material);
+	SceneTree::get_singleton()
+		->get_current_scene()
+		->get_viewport()
+		->add_child(debug_im);
+	return debug_im;
+}
+
 void RoadLinesData::process_lines(
 	std::unordered_map<uint32_t, std::vector<Vector3> >
 		&road_lines_nodes_hash,
 	std::vector<Vector3> &road_lines_nodes)
 {
+	int i;
+	get_debug_node();
+	debug_im->clear();
 	index_lines(road_lines_nodes_hash, road_lines_nodes);
-	insert_close_points(road_lines_nodes);
+	debug_im->begin(Mesh::PRIMITIVE_LINES);
+	for (i = 0; i < (int)road_lines_nodes.size(); i++) {
+		debug_im->set_color(Color(0.1f, 0.6f, 0.6f, 1.0f));
+		debug_im->add_vertex(road_lines_nodes[i]);
+		debug_im->set_color(Color(0.1f, 0.6f, 0.6f, 1.0f));
+		debug_im->add_vertex(road_lines_nodes[i] +
+				     Vector3(0.0f, 200.0f, 0.0f));
+	}
+	debug_im->end();
+	insert_close_points(road_lines_nodes, 160.0f);
 	update_road_lines_nodes(road_lines_nodes);
 	dump_road_lines(road_lines_nodes);
 }