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/io/marshalls.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 "buildings_data.h"
#include "editor_event.h"
#include "base_data.h"
#include "road_lines_data.h"

struct CLine {
	struct RoadLinesData::road_line line;
};
struct CLineIndex {
	struct RoadLinesData::road_line_index index;
};

class LinesAccessor {
public:
	LinesAccessor()
	{
		BaseData::get_singleton()->get().component<CLine>();
	}
	const flecs::world &get() const
	{
		return BaseData::get_singleton()->get();
	}
	flecs::world &get()
	{
		return BaseData::get_singleton()->get();
	}
	template <typename F> void each(F &&func) const
	{
		get().each<F>(func);
	}
	inline flecs::entity lookup(const String &key) const
	{
		String ename = "line:" + key;
		flecs::entity e = get().lookup(ename.ascii().ptr());
		if (!e.is_valid())
			print_line("can't find: " + key);
		assert(e.is_valid());
		return e;
	}
	inline flecs::entity lookup_create(const String &key) const
	{
		String ename = "line:" + key;
		flecs::entity e = get().entity(ename.ascii().ptr());
		assert(e.is_valid());
		return e;
	}
	bool has_entity(const String &key) const
	{
		String ename = "line:" + key;
		flecs::entity e = get().lookup(ename.ascii().ptr());
		return e.is_valid();
	}
	inline const struct RoadLinesData::road_line &
	get_line(const String &key) const
	{
		flecs::entity e = lookup(key);
		const struct CLine *cl = e.get<CLine>();
		return cl->line;
	}
	inline const struct RoadLinesData::road_line *
	get_line_ptr(const String &key) const
	{
		flecs::entity e = lookup(key);
		const struct CLine *cl = e.get<CLine>();
		return &cl->line;
	}
	inline struct RoadLinesData::road_line &get_line(const String &key)
	{
		flecs::entity e = lookup(key);
		struct CLine *cl = e.get_mut<CLine>();
		return cl->line;
	}
	inline const struct RoadLinesData::road_line &
	operator[](const String &key) const
	{
		flecs::entity e = lookup(key);
		const struct CLine *cl = e.get<CLine>();
		return cl->line;
	}
	inline void set_line(const String &key,
			     const struct RoadLinesData::road_line &line)
	{
		flecs::entity e = lookup_create(key);
		e.set<CLine>({ line });
		CLine *cl = e.get_mut<CLine>();
		if (cl->line.edges.size() != cl->line.points.size() - 1)
			cl->line.edges.resize(cl->line.points.size() - 1);
	}
	inline void insert_line_point(const String &key, int index,
				      const Transform &xform)
	{
		flecs::entity e = lookup(key);
		struct CLine *cl = e.get_mut<CLine>();
		cl->line.points.insert(cl->line.points.begin() + index, xform);
		if (cl->line.edges.size() != cl->line.points.size() - 1)
			cl->line.edges.resize(cl->line.points.size() - 1);
	}
	inline void erase_line_point(const String &key, int index)
	{
		flecs::entity e = lookup(key);
		struct CLine *cl = e.get_mut<CLine>();
		cl->line.points.erase(cl->line.points.begin() + index);
		if (cl->line.edges.size() != cl->line.points.size() - 1)
			cl->line.edges.resize(cl->line.points.size() - 1);
	}
	inline void set_line_point_position(const String &key, int index,
					    const Vector3 &position)
	{
		flecs::entity e = lookup(key);
		struct CLine *cl = e.get_mut<CLine>();
		cl->line.points[index].origin = position;
	}
	inline int get_line_points_count(const String &key) const
	{
		flecs::entity e = lookup(key);
		const struct CLine *cl = e.get<CLine>();
		return cl->line.points.size();
	}
	inline const Vector3 &get_line_point_position(const String &key,
						      int index) const
	{
		flecs::entity e = lookup(key);
		const struct CLine *cl = e.get<CLine>();
		return cl->line.points[index].origin;
	}
	inline void clear_line_segments(const String &key)
	{
		flecs::entity e = lookup(key);
		struct CLine *cl = e.get_mut<CLine>();
		cl->line.segments.clear();
	}
	void update_line_segments(const String &key)
	{
		int i;
		flecs::entity e = lookup(key);
		clear_line_segments(key);
		struct CLine *cl = e.get_mut<CLine>();
		cl->line.segments.resize(get_line_points_count(key) - 1);
		float offset = 0.0f;
		for (i = 0; i < (int)get_line_points_count(key) - 1; i++) {
			struct RoadLinesData::line_segment segment;
			segment.p1 = get_line_point_position(key, i);
			segment.p2 = get_line_point_position(key, i + 1);
			segment.length = segment.p1.distance_to(segment.p2);
			segment.dir = (segment.p2 - segment.p1).normalized();
			Vector3 side = segment.dir.cross(Vector3(0, 1, 0));
			side.y = 0;
			segment.tangent = side.normalized();
			segment.offset = offset;
			cl->line.segments[i] = segment;
			offset += segment.length;
		}
	}
	void set_line_metadata(const String &key, const Dictionary &metadata)
	{
		flecs::entity e = lookup(key);
		struct CLine *cl = e.get_mut<CLine>();
		cl->line.metadata = metadata;
	}

	inline bool has(const String &key)
	{
		return has_entity(key);
	}
	void get_key_list(List<String> *keys)
	{
		// ugly as fuck
		get().each([keys](flecs::entity e, const CLine &cl) {
			String name(e.name());
			if (name.begins_with("line:")) {
				name = name.substr(5, -1);
				keys->push_back(name);
			}
		});
	}
	const String &get_next(const String &key)
	{
		List<String> keys;
		get_key_list(&keys);
		List<String>::Element *e = keys.find(key);
		assert(e);
		e = e->next();
		assert(e);
		return e->get();
	}
	void erase(const String &key)
	{
		flecs::entity e = lookup(key);
		e.destruct();
	}
};

class IndexAccessor {
public:
	IndexAccessor()
	{
		BaseData::get_singleton()->get().component<CLineIndex>();
	}
	inline const struct RoadLinesData::road_line_index &
	operator[](const String &key) const
	{
		flecs::entity e = lookup(key);
		const struct CLineIndex *cl = e.get<CLineIndex>();
		return cl->index;
	}
	const flecs::world &get() const
	{
		return BaseData::get_singleton()->get();
	}
	flecs::world &get()
	{
		return BaseData::get_singleton()->get();
	}
	template <typename F> void each(F &&func) const
	{
		get().each<F>(func);
	}
	inline flecs::entity lookup(const String &key) const
	{
		String ename = "line:" + key;
		flecs::entity e = get().lookup(ename.ascii().ptr());
		if (!e.is_valid())
			print_line("can't find: " + key);
		assert(e.is_valid());
		return e;
	}
	inline void clear_all_line_indices()
	{
		BaseData::get_singleton()->get_singleton()->get().each(
			[](CLineIndex &cl) { cl.index.indices.clear(); });
	}
	inline void clear_line_indices(const String &key)
	{
		flecs::entity e = lookup(key);
		struct CLineIndex *cl = e.get_mut<CLineIndex>();
		assert(cl);
		cl->index.indices.clear();
	}
	inline void add_line_index(const String &key, int id)
	{
		flecs::entity e = lookup(key);
		struct CLineIndex *cl = e.get_mut<CLineIndex>();
		cl->index.indices.push_back(id);
	}
	inline void insert_line_index(const String &key, int index, int id)
	{
		flecs::entity e = lookup(key);
		struct CLineIndex *cl = e.get_mut<CLineIndex>();
		cl->index.indices.insert(cl->index.indices.begin() + index, id);
	}
	inline void
	set_index(const String &key,
		  const struct RoadLinesData::road_line_index &index)
	{
		flecs::entity e = lookup_create(key);
		e.set<CLineIndex>({ index });
	}
	inline flecs::entity lookup_create(const String &key) const
	{
		String ename = "line:" + key;
		flecs::entity e = get().entity(ename.ascii().ptr());
		assert(e.is_valid());
		return e;
	}
};

static LinesAccessor lines;
static IndexAccessor indices;

ImmediateGeometry *RoadLinesData::debug_im = nullptr;
static Ref<Material> debug_material;
RoadLinesData::RoadLinesData()
	: initialized(false)
{
	load_data();
}
RoadLinesData *RoadLinesData::singleton = nullptr;
const struct RoadLinesData::road_line &
RoadLinesData::get_line(const String &key) const
{
	return ::lines[key];
}
const RoadLinesData::road_line &RoadLinesData::lines(const String &key) const
{
	return ::lines[key];
}
const RoadLinesData::road_line_index &
RoadLinesData::indices(const String &key) const
{
	return ::indices[key];
}
void RoadLinesData::set_line(const String &key, const road_line &line)
{
	::lines.set_line(key, line);
}
bool RoadLinesData::has_line(const String &key)
{
	return ::lines.has(key);
}
void RoadLinesData::insert_line_point(const String &key, int index,
				      const Transform &xform)
{
	::lines.insert_line_point(key, index, xform);
}
void RoadLinesData::erase_line_point(const String &key, int index)
{
	::lines.erase_line_point(key, index);
}
void RoadLinesData::set_line_point_position(const String &key, int index,
					    const Vector3 &position)
{
	::lines.set_line_point_position(key, index, position);
}
void RoadLinesData::clear_all_line_indices()
{
	::indices.clear_all_line_indices();
}
void RoadLinesData::clear_line_indices(const String &key)
{
	::indices.clear_line_indices(key);
}
void RoadLinesData::set_line_metadata(const String &key,
				      const Dictionary &metadata)
{
	::lines.set_line_metadata(key, metadata);
}
RoadLinesData *RoadLinesData::get_singleton()
{
	if (!singleton)
		singleton = memnew(RoadLinesData);
	assert(singleton->initialized);
	return singleton;
}
RoadLinesData::~RoadLinesData()
{
#if 0
	if (debug_im) {
		memdelete(debug_im);
		debug_im = nullptr;
	}
#endif
}
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::get_lines_key_list(List<String> *keys)
{
	assert(initialized);
	::lines.get_key_list(keys);
	assert(!keys->empty());
}
const String &RoadLinesData::get_next_line(const String &key)
{
	return ::lines.get_next(key);
}
void RoadLinesData::erase_line(const String &key)
{
	return ::lines.erase(key);
}
void RoadLinesData::load_data()
{
	int i;
	ConfigFile config;
	Error result = config.load("res://config/stream.conf");
	ERR_FAIL_COND_MSG(result != OK, "Failed to load config");
	assert(result == OK);
	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: " + road_lines_path +
						": " + 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());
		Array edges = entry.get("edges", 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.edges.resize(edges.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)edges.size(); i++) {
			const Dictionary &d = edges[i];
			rline.edges[i].from_dict(rline.edges[i], d);
		}
		// TODO: wtf is flags?
		rline.lanes = lanes;
		set_line(key, rline);
		RoadLinesData::road_line_index index;
		::indices.set_index(key, index);
		e = e->next();
	}
	{
		List<String> tkeys;
		::lines.get_key_list(&tkeys);
		assert(!tkeys.empty());
	}
	initialized = true;
}
void RoadLinesData::save_data()
{
	int i;
	ConfigFile config;
	ConfigFile line_data;
	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, edges, indices;
		points.resize(lines(e->get()).points.size());
		edges.resize(lines(e->get()).edges.size());
		for (i = 0; i < (int)lines(e->get()).points.size(); i++)
			points[i] = to_string(lines(e->get()).points[i]);
		for (i = 0; i < (int)lines(e->get()).edges.size(); i++)
			edges[i] = lines(e->get()).edges[i].to_dict();
		indices.resize(this->indices(e->get()).indices.size());
		for (i = 0; i < (int)this->indices(e->get()).indices.size();
		     i++)
			indices[i] = this->indices(e->get()).indices[i];
		pvalues["points"] = points;
		pvalues["edges"] = edges;
		// 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()));
	}
#if 0
	String road_lines_path_conf = road_lines_path.replace(".json", ".bin");
	if (FileAccess::exists(road_lines_path_conf)) {
		DirAccess *dir = DirAccess::open("res:///", &err);
		assert(dir && err == OK);
		err = dir->copy(
			road_lines_path_conf,
			road_lines_path_conf + "." +
				String::num(
					Time::get_singleton()
						->get_unix_time_from_system()));
	}
	FileAccess *fd =
		FileAccess::open(road_lines_path_conf, FileAccess::WRITE, &err);
	assert(err == OK);
	int len;
	err = encode_variant(output, nullptr, len, false);
	assert(err == OK);
	PoolVector<uint8_t> buffer;
	buffer.resize(len);
	PoolVector<uint8_t>::Write w = buffer.write();
	err = encode_variant(output, w.ptr(), len, false);
	assert(err == OK);
	fd->store_32(len);
	fd->store_buffer(w.ptr(), len);
	fd->close();
	line_data.set_value("line_data", "lines", output);
	line_data.save(road_lines_path_conf);
#endif
	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(
	const String &key,
	std::unordered_map<uint32_t, std::vector<Vector3> >
		&road_lines_nodes_hash,
	std::vector<Vector3> &road_lines_nodes)
{
	int i, j;
	::indices.clear_line_indices(key);
	for (i = 0; i < (int)::lines.get_line_points_count(key); i++) {
		Vector3 pt = ::lines.get_line_point_position(key, i);
		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();
		::indices.add_line_index(key, 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;
	get_road_lines_key_list(&keys);
	List<String>::Element *e = keys.front();
	while (e) {
		String rkey = e->get();
		clear_line_indices(rkey);
		e = e->next();
	}
	e = keys.front();
	while (e) {
		String rkey = e->get();
		road_lines_curve_index(rkey, 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->indices(road).indices[pos];
	return idx;
}
void RoadLinesData::create_segments(const String &road,
				    std::vector<int> &segments)
{
	int i;
	for (i = 0; i < (int)::indices[road].indices.size() - 1; i++) {
		segments.push_back(i);
		segments.push_back(i + 1);
	}
}
/* add close points on each line to the line */
// FIXME: used?
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 */
					::indices.insert_line_index(rkey, 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;

	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 (::indices[k].indices.size() < 2)
			continue;
		if (::indices[r].indices.size() < 2)
			continue;
		for (i = 0; i < (int)::indices[k].indices.size() - 1; i++) {
			for (j = 0; j < (int)::indices[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 = ::indices[k].indices[i];
					int idx_a2 =
						::indices[k].indices[i + 1];
					int idx_b1 = ::indices[k].indices[j];
					int idx_b2 =
						::indices[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(::indices[k].indices.begin(),
					 ::indices[k].indices.end(),
					 nidx) == ::indices[k].indices.end());
			assert(std::find(::indices[r].indices.begin(),
					 ::indices[r].indices.end(),
					 nidx) == ::indices[r].indices.end());
			::indices.insert_line_index(k, i + 1, nidx);
			::indices.insert_line_index(r, j + 1, nidx);
		}
	}
}
void RoadLinesData::dump_road_lines(const std::vector<Vector3> &road_lines_nodes)
{
#if 0
	int i;
	List<String> keys;
	get_road_lines_key_list(&keys);
	List<String>::Element *e = keys.front();
	while (e) {
		String rkey = e->get();
		struct RoadLinesData::road_line &pt = ::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();
	}
#endif
}
ImmediateGeometry *RoadLinesData::get_debug_node()
{
	if (!debug_im) {
		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()
			->call_deferred("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)
{
	index_lines(road_lines_nodes_hash, road_lines_nodes);
	insert_close_points(road_lines_nodes, 160.0f);
	update_road_lines_nodes(road_lines_nodes);
	dump_road_lines(road_lines_nodes);
}
void RoadLinesData::set_debug_flags(int debug_flags)
{
	this->debug_flags = debug_flags;
}
int RoadLinesData::get_debug_flags() const
{
	return debug_flags;
}

void RoadLinesData::update_line_segments(const String &line)
{
	::lines.update_line_segments(line);
}

Vector3 RoadLinesData::get_point_by_offsets(const String &line,
					    float dir_offset,
					    float normal_offset)
{
	Vector3 ret;
	int i;
	assert(::lines.has(line));
	print_verbose("line: " + line +
		      " line_offset: " + String::num(dir_offset) +
		      " normal_offset: " + String::num(normal_offset));
	float n_offset = dir_offset;
	int selected_segment = 0;
	for (i = 0; i < (int)::lines[line].segments.size(); i++) {
		const struct line_segment *segment = &::lines[line].segments[i];
		if (n_offset < segment->length) {
			selected_segment = i;
			break;
		}
		n_offset -= segment->length;
	}
	print_verbose("offset: " + String::num(n_offset));
	ret = ::lines[line].segments[selected_segment].p1 +
	      ::lines[line].segments[selected_segment].dir * n_offset +
	      ::lines[line].segments[selected_segment].tangent * normal_offset;
	print_verbose("data: " + (ret.operator String()));
	return ret;
}