streaming_world/src/modules/stream/road_lines_data.cpp

#undef NDEBUG
#include <cassert>
#include <algorithm>
#include <vector>
#include <unordered_map>
#include <core/io/config_file.h>
#include <core/io/json.h>
#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;
	ConfigFile config;
	Error result = config.load("res://config/stream.conf");
	ERR_FAIL_COND_MSG(result != OK, "Failed to load config");
	road_lines_path = config.get_value("lines", "road_lines_path");
	String road_lines_path = config.get_value("lines", "road_lines_path");
	String road_lines_json =
		FileAccess::get_file_as_string(road_lines_path);
	Variant json_v;
	String es;
	int eline;
	Error status = JSON::parse(road_lines_json, json_v, es, eline);
	ERR_FAIL_COND_MSG(status != OK, "Can't parse json: " + es +
						" at line: " + itos(eline));

	Dictionary json = json_v;
	List<Variant> keys;
	json.get_key_list(&keys);
	List<Variant>::Element *e = keys.front();
	while (e) {
		String key = e->get();
		struct road_line rline;
		Dictionary entry = json.get(key, Dictionary());
		Array points = entry.get("points", Array());
		Array indices = entry.get("indices", Array());
		rline.metadata = entry.get("metadata", Dictionary());
		int lanes = entry.get("lanes", -1);
		int pattern = entry.get("pattern", -1);
		rline.pattern = pattern;
		rline.points.resize(points.size());
		rline.indices.resize(indices.size());
		for (i = 0; i < (int)points.size(); i++) {
			String point_s = points[i];
			rline.points[i] = from_string<Transform>(point_s);
		}
		for (i = 0; i < (int)indices.size(); i++) {
			int index = indices[i];
			rline.indices[i] = index;
		}
		// TODO: wtf is flags?
		rline.lanes = lanes;
		lines[key] = rline;
		e = e->next();
	}
}

RoadLinesData *RoadLinesData::singleton = nullptr;
RoadLinesData *RoadLinesData::get_singleton()
{
	if (!singleton)
		singleton = memnew(RoadLinesData);
	return singleton;
}

RoadLinesData::~RoadLinesData()
{
	if (debug_im) {
		memdelete(debug_im);
		debug_im = nullptr;
	}
}

void RoadLinesData::cleanup()
{
	memdelete(singleton);
	singleton = nullptr;
}

String RoadLinesData::get_road_lines_path()
{
	return road_lines_path;
}

void RoadLinesData::get_road_lines_key_list(List<String> *keys)
{
	List<String> line_keys;
	lines.get_key_list(&line_keys);
	List<String>::Element *e = line_keys.front();
	keys->clear();
	while (e) {
		const String &key = e->get();
		if (key.ends_with("_road"))
			keys->push_back(key);
		e = e->next();
	}
}
void RoadLinesData::save_data()
{
	int i;
	ConfigFile config;
	Error result = config.load("res://config/stream.conf");
	ERR_FAIL_COND_MSG(result != OK, "Failed to load config");
	String road_lines_path = config.get_value("road", "road_lines_path");
	String road_lines_json =
		FileAccess::get_file_as_string(road_lines_path);
	Dictionary output;
	List<String> keys;
	lines.get_key_list(&keys);
	List<String>::Element *e = keys.front();
	while (e) {
		Dictionary pvalues;
		Array points, indices;
		points.resize(lines[e->get()].points.size());
		for (i = 0; i < (int)lines[e->get()].points.size(); i++)
			points[i] = to_string(lines[e->get()].points[i]);
		indices.resize(lines[e->get()].indices.size());
		for (i = 0; i < (int)lines[e->get()].indices.size(); i++)
			indices[i] = lines[e->get()].indices[i];
		pvalues["points"] = points;
		// pvalues["indices"] = indices;
		pvalues["metadata"] = lines[e->get()].metadata;
		pvalues["lanes"] = lines[e->get()].lanes;
		pvalues["pattern"] = lines[e->get()].pattern;
		output[e->get()] = pvalues;
		e = e->next();
	}
	print_verbose(JSON::print(output, "\t", false));
	Error err = OK;
	if (FileAccess::exists(road_lines_path)) {
		DirAccess *dir = DirAccess::open("res:///", &err);
		assert(dir && err == OK);
		err = dir->copy(
			road_lines_path,
			road_lines_path + "." +
				String::num(
					Time::get_singleton()
						->get_unix_time_from_system()));
	}
	FileAccess *fd =
		FileAccess::open(road_lines_path, FileAccess::WRITE, &err);
	if (err == OK) {
		fd->store_string(JSON::print(output, "\t", false));
		fd->close();
	}
}
uint32_t RoadLinesData::road_lines_hash(const Vector3 &v)
{
	int x = (int)(v.x / 100);
	int y = (int)(v.y / 100);
	int z = (int)(v.z / 100);
	return x ^ (y * 100) ^ (z * 10000);
}

void RoadLinesData::road_lines_curve_index(
	struct RoadLinesData::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();
	for (i = 0; i < (int)rline.points.size(); i++) {
		Vector3 pt = rline.points[i].origin;
		int pt_hash = road_lines_hash(pt);
		if (road_lines_nodes_hash.find(pt_hash) !=
		    road_lines_nodes_hash.end()) {
			bool ok = true;
			for (j = 0;
			     j < (int)road_lines_nodes_hash[pt_hash].size();
			     j++) {
				const Vector3 &xpt =
					road_lines_nodes_hash[pt_hash][j];
				if (xpt.distance_squared_to(pt) < 160) {
					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 };
		}
		std::vector<Vector3>::iterator it = std::find(
			road_lines_nodes.begin(), road_lines_nodes.end(), pt);
		assert(it != road_lines_nodes.end());
		int index = it - road_lines_nodes.begin();
		rline.indices.push_back(index);
	}
}
void RoadLinesData::index_lines(
	std::unordered_map<uint32_t, std::vector<Vector3> >
		&road_lines_nodes_hash,
	std::vector<Vector3> &road_lines_nodes)
{
	List<String> keys;
	RoadLinesData *rld = RoadLinesData::get_singleton();
	rld->get_road_lines_key_list(&keys);
	List<String>::Element *e = keys.front();
	while (e) {
		String rkey = e->get();
		struct RoadLinesData::road_line &pt = rld->lines[rkey];
		pt.indices.clear();
		e = e->next();
	}
	e = keys.front();
	while (e) {
		String rkey = e->get();
		struct RoadLinesData::road_line &pt = rld->lines[rkey];
		road_lines_curve_index(pt, road_lines_nodes_hash,
				       road_lines_nodes);
		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++) {
		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,
					float distance_squared)
{
	int i;
	List<String> 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 += 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];
				std::vector<Vector3> seg = { p1, p2 };
				Vector3 closest =
					Geometry::get_closest_point_to_segment(
						p3, seg.data());
				/* 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();
		}
	}
}

void RoadLinesData::update_road_lines_nodes(
	std::vector<Vector3> &road_lines_nodes)
{
	List<String> keys;
	RoadLinesData *rld = RoadLinesData::get_singleton();

	rld->get_road_lines_key_list(&keys);
	std::unordered_map<uint32_t, std::tuple<String, String> > kcmp;
	{
		List<String>::Element *k = keys.front();
		List<String>::Element *r = keys.front();
		while (k) {
			String kkey = k->get();
			uint32_t kkey_hash = kkey.hash();
			while (r) {
				String rkey = r->get();
				uint32_t rkey_hash = rkey.hash();
				uint32_t key = kkey_hash ^ rkey_hash;
				uint32_t key2 = rkey_hash ^ kkey_hash;
				if (kcmp.find(key) == kcmp.end() &&
				    kcmp.find(key2) == kcmp.end())
					kcmp[key] = std::make_tuple(k->get(),
								    r->get());
				r = r->next();
			}
			k = k->next();
		}
	}
	using checks_tuple =
		std::tuple<String, int, int, int, String, int, int, int>;
	std::unordered_map<uint32_t, checks_tuple> checks;
	std::unordered_map<uint32_t, checks_tuple>::iterator checks_it;
	std::unordered_map<uint32_t, std::tuple<String, String> >::iterator it;
	for (it = kcmp.begin(); it != kcmp.end(); it++) {
		int i, j;
		std::tuple<String, String> data = kcmp[it->first];
		const String &k = std::get<0>(data);
		const String &r = std::get<1>(data);
		if (rld->lines[k].indices.size() < 2)
			continue;
		if (rld->lines[r].indices.size() < 2)
			continue;
		for (i = 0; i < (int)rld->lines[k].indices.size() - 1; i++) {
			for (j = 0; j < (int)rld->lines[k].indices.size() - 1;
			     j++) {
				uint32_t key = k.hash() ^ i ^ r.hash() ^ j ^
					       2147483137;
				uint32_t key2 = r.hash() ^ j ^ k.hash() ^ i ^
						2147463167;
				if (checks.find(key) == checks.end() &&
				    checks.find(key2) == checks.end()) {
					int idx_a1 = rld->lines[k].indices[i];
					int idx_a2 =
						rld->lines[k].indices[i + 1];
					int idx_b1 = rld->lines[k].indices[j];
					int idx_b2 =
						rld->lines[k].indices[j + 1];
					std::vector<int> cmp1 = { idx_a1,
								  idx_a2 };
					if (std::find(cmp1.begin(), cmp1.end(),
						      idx_b1) != cmp1.end())
						continue;
					if (std::find(cmp1.begin(), cmp1.end(),
						      idx_b2) != cmp1.end())
						continue;
					checks[key] = std::make_tuple(
						k, i, idx_a1, idx_a2, r, j,
						idx_b1, idx_b2);
				}
			}
		}
	}
	/*
	for ch in checks.values():
		var k = ch[0]
		var i1 = ch[1]
		var idx_a1 = ch[2]
		var idx_a2 = ch[3]
		var r = ch[4]
		var j1 = ch[5]
		var idx_b1 = ch[6]
		var idx_b2 = ch[7]
		var p_a1 = road_lines_nodes[idx_a1]
		var p_a2 = road_lines_nodes[idx_a2]
		var p_b1 = road_lines_nodes[idx_b1]
		var p_b2 = road_lines_nodes[idx_b2]
		var px = Geometry.get_closest_points_between_segments(p_a1, p_a2, p_b1, p_b2)
		var d = px[0].distance_squared_to(px[1])
		if d < 160:
			var pxt = px[0].linear_interpolate(px[1], 0.5)
			var nidx = road_lines_nodes.size()
			road_lines_nodes.push_back(pxt)
			var il = road_lines[k].indices.size()
			assert(!nidx in road_lines[k].indices)
			assert(!nidx in road_lines[r].indices)
			road_lines[k].indices.insert(i1 + 1, nidx)
			road_lines[r].indices.insert(j1 + 1, nidx)
##end
		*/
	for (checks_it = checks.begin(); checks_it != checks.end();
	     checks_it++) {
		String k = std::get<0>(checks_it->second);
		int i = std::get<1>(checks_it->second);
		int idx_a1 = std::get<2>(checks_it->second);
		int idx_a2 = std::get<3>(checks_it->second);
		String r = std::get<4>(checks_it->second);
		int j = std::get<5>(checks_it->second);
		int idx_b1 = std::get<6>(checks_it->second);
		int idx_b2 = std::get<7>(checks_it->second);
		Vector3 p_a1 = road_lines_nodes[idx_a1];
		Vector3 p_a2 = road_lines_nodes[idx_a2];
		Vector3 p_b1 = road_lines_nodes[idx_b1];
		Vector3 p_b2 = road_lines_nodes[idx_b2];
		Vector3 px, px2;
		Geometry::get_closest_points_between_segments(p_a1, p_a2, p_b1,
							      p_b2, px, px2);
		float d = px.distance_squared_to(px2);
		if (d < 160) {
			Vector3 pxt = px.linear_interpolate(px2, 0.5f);
			int nidx = road_lines_nodes.size();
			road_lines_nodes.push_back(pxt);
			//				int il = (int)road_lines[k].indices.size();
			assert(std::find(rld->lines[k].indices.begin(),
					 rld->lines[k].indices.end(),
					 nidx) == rld->lines[k].indices.end());
			assert(std::find(rld->lines[r].indices.begin(),
					 rld->lines[r].indices.end(),
					 nidx) == rld->lines[r].indices.end());
			rld->lines[k].indices.insert(
				rld->lines[k].indices.begin() + i + 1, nidx);
			rld->lines[r].indices.insert(
				rld->lines[k].indices.begin() + j + 1, nidx);
		}
	}
}
void RoadLinesData::dump_road_lines(const std::vector<Vector3> &road_lines_nodes)
{
	int i;
	List<String> keys;
	RoadLinesData *rld = RoadLinesData::get_singleton();
	rld->get_road_lines_key_list(&keys);
	List<String>::Element *e = keys.front();
	while (e) {
		String rkey = e->get();
		struct RoadLinesData::road_line &pt = rld->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());
		}
		e = e->next();
	}
}

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);
	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);
}