academy2/modules/world/roads.cpp

#include <cassert>
#include <core/resource.h>
#include <core/os/os.h>
#include <core/sort_array.h>
#include <scene/resources/packed_scene.h>
#include <scene/main/viewport.h>
#include <scene/3d/camera.h>
#include <scene/3d/physics_body.h>
#include <scene/3d/collision_shape.h>
#include "roads.h"

enum {
	FLAGS_SIDEWALK = (1 << 0),
	FLAGS_INTERSECTION = (1 << 1),
	FLAGS_WALL = (1 << 2),
};

void Roads::_bind_methods()
{
	ClassDB::bind_method(D_METHOD("add_scene_element", "root", "surface", "p2", "shape"), &Roads::add_scene_element);
}
void Roads::_get_property_list(List<PropertyInfo> *p_list) const
{
	p_list->push_back(PropertyInfo(Variant::OBJECT, "road_data", PROPERTY_HINT_RESOURCE_TYPE, "PackedScene"));
	p_list->push_back(PropertyInfo(Variant::OBJECT, "curve", PROPERTY_HINT_RESOURCE_TYPE, "Curve"));
	p_list->push_back(PropertyInfo(Variant::OBJECT, "noise", PROPERTY_HINT_RESOURCE_TYPE, "FastNoiseLite"));
}
bool Roads::_get(const StringName &p_name, Variant &r_ret) const
{
	if (p_name == "road_data") {
		r_ret = road_data;
		return true;
	} else if (p_name == "curve") {
		r_ret = curve;
		return true;
	} else if (p_name == "noise") {
		r_ret = noise;
		return true;
	}
	return false;
}
bool Roads::_set(const StringName &p_name, const Variant &p_value)
{
	bool update = false;
	if (p_name == "road_data") {
		road_data = p_value;
		update = true;
	} else if (p_name == "curve") {
		curve = p_value;
		update = true;
	} else if (p_name == "noise") {
		noise = p_value;
		update = true;
	}
	if (update) {
		update_all();
		_change_notify();
	}

	return update;
}
void Roads::update_all()
{
	int i;
	RoadGrid *rg = RoadsData::get_singleton()->get_road_grid();
	if (road_data.is_valid()) {
		Node *tmp = road_data->instance();
		for (i = 0; i < tmp->get_child_count(); i++) {
			Node *c = tmp->get_child(i);
			MeshInstance *mi = Object::cast_to<MeshInstance>(c);
			if (mi) {
				String name = mi->get_name();
				Ref<ArrayMesh> mesh = mi->get_mesh();
				Array data = mesh->surface_get_arrays(0);
				mesh_data[name] = data;
				if (name == "road_main")
					mat = mesh->surface_get_material(0);
			}
		}
		if (curve.is_valid() && noise.is_valid()) {
			curve->bake();
			RoadsData::get_singleton()->set_noise(noise);
			RoadsData::get_singleton()->set_curve(curve);
			rg->build(curve, noise);
		}
		printf("vertices: %d\n", rg->get_diagram_vertex_count());
		printf("edges: %d\n", rg->get_map_hedges_count());
		rg->setup_vshapes();
	}
}

Roads::Roads()
{
	body = memnew(StaticBody);
}
Roads::~Roads()
{
	memdelete(body);
}
Vector3 Roads::calculate_offsets(const Array &data) const
{
	float minx = 0.0f, maxx = 0.0f, minz = 0.0f, maxz = 0.0f;
	PoolVector<Vector3> verts = data[Mesh::ARRAY_VERTEX];
	int i;
	Vector3 ret;
	for (i = 0; i < verts.size(); i++) {
		if (minx > verts.read()[i].x)
			minx = verts.read()[i].x;
		if (maxx < verts.read()[i].x)
			maxx = verts.read()[i].x;
		if (minz > verts.read()[i].z)
			minz = verts.read()[i].z;
		if (maxz < verts.read()[i].z)
			maxz = verts.read()[i].z;
	}
	ret.x = fabsf(maxx - minx);
	ret.z = fabsf(maxz - minz);
	return ret;
}
Vector3 Roads::quadratic_bezier(const Vector3 &p0, const Vector3 &p1,
		const Vector3 &p2, float t) const
{
	Vector3 q0 = p0.linear_interpolate(p1, t);
	Vector3 q1 = p1.linear_interpolate(p2, t);
	Vector3 r = q0.linear_interpolate(q1, t);
	return r;
}
void Roads::all_offsets()
{
	const String *k;
	for (k = mesh_data.next(NULL); k; k = mesh_data.next(k))
		offsets[*k] = calculate_offsets(mesh_data[*k]);
}
PoolVector<String> Roads::build_item_list(float width, int flags, float sidewalk_width) const
{
	PoolVector<String> ret;
	float tx = 0.0f;
	while (tx < width) {
		ret.push_back("road_main");
		tx += offsets["road_main"].x;
	}
	if (flags & FLAGS_SIDEWALK) {
		ret.push_back("sidewalk_start");
		tx = 0.0f;
		while (tx < sidewalk_width) {
			ret.push_back("sidewalk");
			tx += offsets["sidewalk"].x;
		}
		ret.push_back("sidewalk_end");
	}
	if (flags & FLAGS_WALL)
		ret.push_back("wall");
	return ret;
}
Ref<Curve3D> Roads::build_curve(Vector3 p1, Vector3 p2, Vector3 p3, float total_width) const
{
	Ref<Curve3D> curve3;
	curve3.instance();
	Vector3 dir1 = (p1 - p2).normalized();
	Vector3 dir2 = (p3 - p2).normalized();
	float qt = 0.0f;
	curve3->add_point(p1);
	while (qt <= 1.0f) {
		Vector3 p = quadratic_bezier(p2 + dir1 * total_width * 1.8f, p2, p2 + dir2 * total_width * 1.8f, qt);
		curve3->add_point(p.snapped(Vector3(0.1, 0.1, 0.1)));
		qt += 0.1f;
	}
	curve3->add_point(p3);
	curve3->set_bake_interval(4.0f);
	assert(curve3->get_baked_length() > 0);
	return curve3;
}
inline float Roads::get_total_width(float width, int flags, float sidewalk_width)
{
	int i;
	float total_width = 0.0f;
	PoolVector<String> parts_list = build_item_list(width, flags, sidewalk_width);
	for (i = 0; i < parts_list.size(); i++)
		total_width += offsets[parts_list[i]].x;
	return total_width;
}

inline Ref<Curve3D> Roads::get_curve(const PoolVector<Vector3> &points, float width, int flags, float sidewalk_width)
{
	float total_width = get_total_width(width, flags, sidewalk_width);
	Ref<Curve3D> curve = build_curve(points[0], points[1], points[2], total_width);
	assert(!curve.is_null());
	return curve;
}

Array Roads::curve_mesh(const PoolVector<Vector3> &points,
		Ref<Curve3D> curve3,
		float total_width1,
		float total_width2,
		const PoolVector<String> &parts_list1,
		const PoolVector<String> &parts_list2,
		int flags,
		float sidewalk_width)
{
	float tx = 0.0f, t = 0.0f, l;
	int i;
	Array ret;
	assert(!curve3.is_null());
	assert(total_width1 >= 3.0f && total_width2 >= 3.0f);
	l = curve3->get_baked_length();
	assert(l > 0.0f);
	PoolVector<Vector3> new_verts, new_normals;
	PoolVector<Vector2> new_uvs;
	PoolVector<int> new_index;
	ret.resize(Mesh::ARRAY_MAX);

	while (t <= l) {
		tx = 0.0f;
		int part = 0;
		while (tx < total_width2) {
			int k;
			Array data = mesh_data[parts_list2[part]];
			int b = new_verts.size();
			PoolVector<Vector3> verts = data[Mesh::ARRAY_VERTEX];
			PoolVector<Vector3> normals = data[Mesh::ARRAY_NORMAL];
			new_verts.resize(b + verts.size());
			new_normals.resize(b + normals.size());
			Transform xform;
			Vector3 offt1, offt2;
			for (k = 0; k < verts.size(); k++) {
				Vector3 base = verts[k];
				float point = t + 2.0 + base.z;
				float right = verts[k].x + tx;
				if (t < l - 2.0) {
					offt1 = curve3->interpolate_baked(point, true);
					offt2 = curve3->interpolate_baked(point + 2.0, true);
					assert(offt1.distance_squared_to(offt2) > 0.0f);
					xform = Transform(Basis(), offt1).looking_at(offt2, Vector3(0.0f, 1.0f, 0.0f));
				} else {
					offt1 = curve3->interpolate_baked(point - 2.0, true);
					offt2 = curve3->interpolate_baked(point, true);
					assert(offt1.distance_squared_to(offt2) > 0.0f);
					xform = Transform(Basis(), offt1).looking_at(offt2, Vector3(0.0f, 1.0f, 0.0f));
					offt1 = offt2;
				}
				xform.origin = Vector3();
				if (right < 0.25f)
					right -= 0.15f;
				Vector3 nvert = offt1 + xform.xform(Vector3(right, verts[k].y, 0.0));
				Vector3 n = xform.xform(normals[k]);
				if (right < 0.15f &&
						((flags & FLAGS_INTERSECTION) != 0) &&
						nvert.distance_squared_to(points[1]) < total_width2 * total_width2 * 2.5f) {
					new_verts.write()[b + k] = points[1];
					new_normals.write()[b + k] = Vector3(0.0f, 1.0f, 0.0f);
				} else {
					new_verts.write()[b + k] = nvert;
					new_normals.write()[b + k] = n;
				}
			}
			new_uvs.append_array(data[Mesh::ARRAY_TEX_UV]);
			int idx = new_index.size();
			PoolVector<int> index = data[Mesh::ARRAY_INDEX];
			new_index.resize(idx + index.size());
			for (k = 0; k < index.size(); k++)
				new_index.write()[idx + k] = index[k] + b;
			tx += offsets[parts_list2[part]].x;
			part += 1;
			if (part >= parts_list2.size())
				break;
		}
		t += 2.0f;
	}
	ret[Mesh::ARRAY_VERTEX] = new_verts;
	ret[Mesh::ARRAY_NORMAL] = new_normals;
	ret[Mesh::ARRAY_TEX_UV] = new_uvs;
	ret[Mesh::ARRAY_INDEX] = new_index;

	return ret;
}

static Ref<ConcavePolygonShape> create_concave_polygon_shape(Vector<Array> surfaces) {
	PoolVector<Vector3> face_points;
	int face_points_size = 0;
	assert(surfaces.size() > 0);

	//find the correct size for face_points
	for (int i = 0; i < surfaces.size(); i++) {
		const Array &surface_arrays = surfaces[i];
		if (surface_arrays.size() == 0) {
			// That surface is empty
			continue;
		}
		// If the surface is not empty then it must have an expected amount of data arrays
		ERR_CONTINUE(surface_arrays.size() != Mesh::ARRAY_MAX);
		assert(surface_arrays.size() == Mesh::ARRAY_MAX);
		PoolVector<int> indices = surface_arrays[Mesh::ARRAY_INDEX];
		face_points_size += indices.size();
	}
	face_points.resize(face_points_size);

	if (face_points_size < 3) {
		assert(0);
		return Ref<ConcavePolygonShape>();
	}

	//copy the points into it
	int face_points_offset = 0;
	for (int i = 0; i < surfaces.size(); i++) {
		const Array &surface_arrays = surfaces[i];
		if (surface_arrays.size() == 0) {
			continue;
		}
		PoolVector<Vector3> positions = surface_arrays[Mesh::ARRAY_VERTEX];
		PoolVector<int> indices = surface_arrays[Mesh::ARRAY_INDEX];

		assert(positions.size() >= 3);
		assert(indices.size() >= 3);
		assert(indices.size() % 3 == 0);

		ERR_FAIL_COND_V(positions.size() < 3, Ref<ConcavePolygonShape>());
		ERR_FAIL_COND_V(indices.size() < 3, Ref<ConcavePolygonShape>());
		ERR_FAIL_COND_V(indices.size() % 3 != 0, Ref<ConcavePolygonShape>());

		int face_points_count = face_points_offset + indices.size();

		{
			PoolVector<Vector3>::Write w = face_points.write();
			PoolVector<int>::Read index_r = indices.read();
			PoolVector<Vector3>::Read position_r = positions.read();

			for (int p = face_points_offset; p < face_points_count; ++p) {
				w[p] = position_r[index_r[p - face_points_offset]];
			}
		}

		face_points_offset += indices.size();
	}

	Ref<ConcavePolygonShape> shape = memnew(ConcavePolygonShape);
	shape->set_faces(face_points);
	return shape;
}

static inline PoolVector<Vector3> make_points(const PoolVector<Vector3> &ipoints)
{
	Vector3 p1 = ipoints[0],
		p2 = ipoints[1],
		p3 = ipoints[2];
	Vector3 m1 = p1 - p2;
	Vector3 m2 = Vector3();
	Vector3 m3 = p3 - p2;
	Vector3 pts[] = {m1, m2, m3};
	int i;
	PoolVector<Vector3> points;
	assert(p1.distance_squared_to(p2) > 2.0f);
	assert(p2.distance_squared_to(p3) > 2.0f);
	assert(p1.distance_squared_to(p3) > 2.0f);
	assert(m1.distance_squared_to(m2) > 2.0f);
	assert(m2.distance_squared_to(m3) > 2.0f);
	assert(m1.distance_squared_to(m3) > 2.0f);
	points.resize(3);
	for (i = 0; i < 3; i++)
		points.write()[i] = pts[i].snapped(Vector3(4.0f, 0.1f, 4.0f));
	return points;
}

int Roads::make_vmesh(Node *root, Ref<Material> mat, Ref<ArrayMesh> mesh, MeshInstance *xmi, RoadMeshData *data)
{
	int i;
	PoolVector<Vector3> points = make_points(data->points);
//	Ref<Curve3D> curve3 = get_curve(points, data->width2, data->flags, data->sidewalk_width);
	assert(!data->vshape->curve3.is_null());
	Array rdata = curve_mesh(points, data->vshape->curve3,
			data->vshape->total_width1,
			data->vshape->total_width2,
			data->vshape->parts_list1,
			data->vshape->parts_list2,
			data->flags,
			data->sidewalk_width);
	Ref<ArrayMesh> mdata = mesh;
	assert(mdata.is_valid());
	mdata->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, rdata);
	assert(mdata->get_surface_count() > 0);
	Vector<Array> surfaces;
	surfaces.push_back(rdata);
	Ref<ConcavePolygonShape> shape = create_concave_polygon_shape(surfaces);
	mdata->surface_set_material(0, mat);
	xmi->set_mesh(mdata);
	call_deferred("add_scene_element", root, xmi, data->points[1], shape);
	return xmi->get_instance_id();
}

void Roads::add_scene_element(Node *root, Node *xnode, const Vector3 &p2, Ref<ConcavePolygonShape> shape)
{
	MeshInstance *xmi = Object::cast_to<MeshInstance>(xnode);
	if (!xmi)
		return;
	root->add_child(xmi);
	Transform xform(Basis(), p2);
	assert(xmi->get_mesh().is_valid() && xmi->get_mesh()->get_surface_count() > 0);
	xmi->set_global_transform(xform);
	StaticBody *sb = memnew(StaticBody);
	CollisionShape *cs = memnew(CollisionShape);
	assert(sb);
	assert(cs);
	sb->add_child(cs);
	assert(!shape.is_null());
	cs->set_shape(shape);
	xmi->call_deferred("add_child", sb);
}

void Roads::process_vshapes()
{
	if (get_viewport() && get_viewport()->get_camera())
		cam_xform = get_viewport()->get_camera()->get_global_transform();
	RoadGrid *rg = RoadsData::get_singleton()->get_road_grid();
	AABB camarea;
	camarea.position = cam_xform.origin;
	camarea.grow_by(550.0f);
	int i;
	List<int> active_vshapes;
	const PoolVector<struct vshape> &vshapes = rg->get_vshapes();
	for (i = 0; i < vshapes.size(); i++) {
		if (active_vshapes.size() > 32)
			break;
		if (vshapes[i].instance >= 0)
			continue;
#if 0
		active_vshapes.push_back(i);
#else
		if (vshapes[i].area.intersects(camarea))
			active_vshapes.push_back(i);
		else if (vshapes[i].area.encloses(camarea))
			active_vshapes.push_back(i);
		else if (camarea.encloses(vshapes[i].area))
			active_vshapes.push_back(i);
		else if (camarea.intersects(vshapes[i].area))
			active_vshapes.push_back(i);
#endif
	}
	List<int>::Element *e;
	for (e = active_vshapes.front(); e; e = e->next()) {
		i = e->get();
		struct vshape &v = rg->get_vshapes().write()[i];
		assert(v.p1.distance_squared_to(v.p2) > 2.0f);
		assert(v.p2.distance_squared_to(v.p3) > 2.0f);
		assert(v.p1.distance_squared_to(v.p3) > 2.0f);
		if (vshapes[i].instance < 0) {
			if (thread.thread.is_started())
				thread.thread.wait_to_finish();
			thread.mat = mat;
			thread.root = this;
			Ref<ArrayMesh> mesh;
			mesh.instance();
			thread.mesh = mesh;
			thread.xmi = memnew(MeshInstance);
			create_data(&v, &thread.data,
					FLAGS_SIDEWALK|FLAGS_INTERSECTION,
					sidewalk_width);
			thread.thread.start(generate_threaded, &thread);
		}
	}
}
void Roads::create_vshape_data(struct vshape *v, int flags, float sidewalk_width)
{
	PoolVector<Vector3> ipoints;
	ipoints.resize(3);
	ipoints.write()[0] = v->p1;
	ipoints.write()[1] = v->p2;
	ipoints.write()[2] = v->p3;
	v->total_width1 = get_total_width(v->depth1, flags, sidewalk_width);
	v->total_width2 = get_total_width(v->depth2, flags, sidewalk_width);
	v->parts_list1 = build_item_list(v->depth1, flags, sidewalk_width);
	v->parts_list2 = build_item_list(v->depth2, flags, sidewalk_width);
	PoolVector<Vector3> points = make_points(ipoints);
	v->curve3 = get_curve(points, v->depth2, flags, sidewalk_width);
	v->curve3a = get_curve(points, v->depth1, flags, sidewalk_width);
	assert(!v->curve3.is_null());
}
void Roads::create_data(struct vshape *v, struct RoadMeshData *data, int flags, float sidewalk_width)
{
	data->points.resize(3);
	data->points.write()[0] = v->p1;
	data->points.write()[1] = v->p2;
	data->points.write()[2] = v->p3;
	data->width1 = v->depth1;
	data->width2 = v->depth2;
	data->flags = flags;
	data->sidewalk_width = sidewalk_width;
	data->vshape = v;
}

void Roads::build_vshape_data()
{
	int i;
	RoadGrid *rg = RoadsData::get_singleton()->get_road_grid();
	PoolVector<struct vshape> &vshapes = rg->get_vshapes();
	for (i = 0; i < vshapes.size(); i++)
		create_vshape_data(&(vshapes.write()[i]), FLAGS_SIDEWALK|FLAGS_INTERSECTION,
				sidewalk_width);
}

void Roads::_notification(int p_what)
{
	switch(p_what) {
	case NOTIFICATION_PROCESS:
		if ((counter % 60) == 0)
			process_vshapes();
		counter++;
		break;
	case NOTIFICATION_READY:
		counter = 0;
		set_process(true);
		add_child(body);
		sidewalk_width = 6.0f;
		all_offsets();
		build_vshape_data();
		break;
	}
}
void Roads::generate_threaded(void *p_userdata)
{
	struct thread_data *data = (struct thread_data *)p_userdata;
	Roads *obj = Object::cast_to<Roads>(data->root);
	int instance = obj->make_vmesh(obj, data->mat, data->mesh, data->xmi, &data->data);
	assert(instance >= 0);
	obj->mutex.lock();

	data->data.vshape->instance = instance;
	obj->mutex.unlock();
}


RoadsData::RoadsData()
{
	rg = memnew(RoadGrid);
}
RoadsData::~RoadsData()
{
	memdelete(rg);
	rg = NULL;
}
static RoadsData *g_roads_data = NULL;
RoadsData* RoadsData::get_singleton()
{
	return g_roads_data;
}

void  RoadsData::create_singleton()
{
	g_roads_data = memnew(RoadsData);
}
void  RoadsData::destroy_singleton()
{
	memdelete(g_roads_data);
	g_roads_data = NULL;
}
RoadGrid *RoadsData::get_road_grid() const
{
	return rg;
}
void RoadsData::_bind_methods()
{
	ClassDB::bind_method(D_METHOD("get_road_grid"), &RoadsData::get_road_grid);
	ClassDB::bind_method(D_METHOD("get_sdf", "x", "y", "z"), &RoadsData::get_sdf);
	ClassDB::bind_method(D_METHOD("get_site_pos", "site"), &RoadsData::get_site_pos);
	ClassDB::bind_method(D_METHOD("get_site_polygon_2d", "site"), &RoadsData::get_site_polygon_2d);
	ClassDB::bind_method(D_METHOD("get_site_polygon_3d", "site"), &RoadsData::get_site_polygon_3d);
	ClassDB::bind_method(D_METHOD("get_here_sites", "position"), &RoadsData::get_here_sites);
	ClassDB::bind_method(D_METHOD("get_site_avg_height", "site"), &RoadsData::get_site_avg_height);
	ClassDB::bind_method(D_METHOD("get_site_border", "site"), &RoadsData::get_site_border);
	ClassDB::bind_method(D_METHOD("site_is_town", "site"), &RoadsData::site_is_town);
	ClassDB::bind_method(D_METHOD("site_is_farm", "site"), &RoadsData::site_is_farm);
	ClassDB::bind_method(D_METHOD("get_site_count"), &RoadsData::get_site_count);
	ClassDB::bind_method(D_METHOD("save_json", "path"), &RoadsData::save_json);
	ClassDB::bind_method(D_METHOD("get_site_type", "site"), &RoadsData::get_site_type);
}
void RoadsData::set_noise(Ref<FastNoiseLite> noise)
{
	this->noise = noise;
}
void RoadsData::set_curve(Ref<Curve> curve)
{
	this->curve = curve;
}
float RoadsData::get_sdf(int x, int y, int z)
{
	if (!curve.is_valid() || !noise.is_valid())
		return (float)y;
	/* will need to fix this for larger world */
	if (sdf_data.has(x * 50000 + z))
		return (float)y - sdf_data[x * 50000 + z];
	float ret;
	float n = curve->interpolate_baked(0.5f + noise->get_noise_2d(x, z) * 0.5f);
	n = CLAMP(n, -1000.0f, 1000.0f);
	/* this is for height value; for caves/tunnels other logic is needed */
	Vector2 ifl = rg->get_influence(x, z, 32.0f);
	if (ifl.x > 0.0f) {
		sdf_mutex.lock();
		if (n <= ifl.y) {
			ret = (float)y - ifl.y - 2.1f;
			sdf_data[x * 50000 + z] = ifl.y + 2.1f;
		} else {
			ret = (float)y - ifl.y - 2.1f;
			sdf_data[x * 50000 + z] = ifl.y + 2.1f;
		}
		sdf_mutex.unlock();
		goto out;
	} else {
		int site = rg->get_site_from_point(x, z);
//		printf("in site %d %d %d\n", site, rg->site_is_town(site), rg->site_is_farm(site));
		if (site >= 0 && (rg->site_is_town(site) || rg->site_is_farm(site))) {
			ret = y - rg->get_site_avg_height(site) - CLAMP(n * 0.1f, -0.05f, 0.05f);
			goto out;
		}
	}
	ret =  y - n;
out:
	return ret;
}

Vector2 RoadsData::get_site_pos(int site)
{
	return rg->get_site_pos(site);
}

PoolVector<Vector2> RoadsData::get_site_polygon_2d(int site)
{
	return rg->get_site_polygon_2d(site);
}

PoolVector<Vector3> RoadsData::get_site_polygon_3d(int site)
{
	return rg->get_site_polygon_3d(site);
}

PoolVector<int> RoadsData::get_here_sites(const Vector3 &position)
{
	return rg->get_here_sites(position);
}
bool RoadsData::site_is_town(int site) const
{
	return rg->site_is_town(site);
}
bool RoadsData::site_is_farm(int site) const
{
	return rg->site_is_farm(site);
}