virtualx-engine/scene/resources/polygon_path_finder.cpp

/*************************************************************************/
/*  polygon_path_finder.cpp                                              */
/*************************************************************************/
/*                       This file is part of:                           */
/*                           GODOT ENGINE                                */
/*                      https://godotengine.org                          */
/*************************************************************************/
/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md).   */
/*                                                                       */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the       */
/* "Software"), to deal in the Software without restriction, including   */
/* without limitation the rights to use, copy, modify, merge, publish,   */
/* distribute, sublicense, and/or sell copies of the Software, and to    */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions:                                             */
/*                                                                       */
/* The above copyright notice and this permission notice shall be        */
/* included in all copies or substantial portions of the Software.       */
/*                                                                       */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,       */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF    */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY  */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,  */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
/*************************************************************************/

#include "polygon_path_finder.h"
#include "core/math/geometry_2d.h"

bool PolygonPathFinder::_is_point_inside(const Vector2 &p_point) const {
	int crosses = 0;

	for (RBSet<Edge>::Element *E = edges.front(); E; E = E->next()) {
		const Edge &e = E->get();

		Vector2 a = points[e.points[0]].pos;
		Vector2 b = points[e.points[1]].pos;

		if (Geometry2D::segment_intersects_segment(a, b, p_point, outside_point, nullptr)) {
			crosses++;
		}
	}

	return crosses & 1;
}

void PolygonPathFinder::setup(const Vector<Vector2> &p_points, const Vector<int> &p_connections) {
	ERR_FAIL_COND(p_connections.size() & 1);

	points.clear();
	edges.clear();

	//insert points

	int point_count = p_points.size();
	points.resize(point_count + 2);
	bounds = Rect2();

	for (int i = 0; i < p_points.size(); i++) {
		points.write[i].pos = p_points[i];
		points.write[i].penalty = 0;

		outside_point.x = i == 0 ? p_points[0].x : (MAX(p_points[i].x, outside_point.x));
		outside_point.y = i == 0 ? p_points[0].y : (MAX(p_points[i].y, outside_point.y));

		if (i == 0) {
			bounds.position = points[i].pos;
		} else {
			bounds.expand_to(points[i].pos);
		}
	}

	outside_point.x += 20.451 + Math::randf() * 10.2039;
	outside_point.y += 21.193 + Math::randf() * 12.5412;

	//insert edges (which are also connections)

	for (int i = 0; i < p_connections.size(); i += 2) {
		Edge e(p_connections[i], p_connections[i + 1]);
		ERR_FAIL_INDEX(e.points[0], point_count);
		ERR_FAIL_INDEX(e.points[1], point_count);
		points.write[p_connections[i]].connections.insert(p_connections[i + 1]);
		points.write[p_connections[i + 1]].connections.insert(p_connections[i]);
		edges.insert(e);
	}

	//fill the remaining connections based on visibility

	for (int i = 0; i < point_count; i++) {
		for (int j = i + 1; j < point_count; j++) {
			if (edges.has(Edge(i, j))) {
				continue; //if in edge ignore
			}

			Vector2 from = points[i].pos;
			Vector2 to = points[j].pos;

			if (!_is_point_inside(from * 0.5 + to * 0.5)) { //connection between points in inside space
				continue;
			}

			bool valid = true;

			for (RBSet<Edge>::Element *E = edges.front(); E; E = E->next()) {
				const Edge &e = E->get();
				if (e.points[0] == i || e.points[1] == i || e.points[0] == j || e.points[1] == j) {
					continue;
				}

				Vector2 a = points[e.points[0]].pos;
				Vector2 b = points[e.points[1]].pos;

				if (Geometry2D::segment_intersects_segment(a, b, from, to, nullptr)) {
					valid = false;
					break;
				}
			}

			if (valid) {
				points.write[i].connections.insert(j);
				points.write[j].connections.insert(i);
			}
		}
	}
}

Vector<Vector2> PolygonPathFinder::find_path(const Vector2 &p_from, const Vector2 &p_to) {
	Vector<Vector2> path;

	Vector2 from = p_from;
	Vector2 to = p_to;
	Edge ignore_from_edge(-1, -1);
	Edge ignore_to_edge(-1, -1);

	if (!_is_point_inside(from)) {
		float closest_dist = 1e20f;
		Vector2 closest_point;

		for (RBSet<Edge>::Element *E = edges.front(); E; E = E->next()) {
			const Edge &e = E->get();
			Vector2 seg[2] = {
				points[e.points[0]].pos,
				points[e.points[1]].pos
			};

			Vector2 closest = Geometry2D::get_closest_point_to_segment(from, seg);
			float d = from.distance_squared_to(closest);

			if (d < closest_dist) {
				ignore_from_edge = E->get();
				closest_dist = d;
				closest_point = closest;
			}
		}

		from = closest_point;
	};

	if (!_is_point_inside(to)) {
		float closest_dist = 1e20f;
		Vector2 closest_point;

		for (RBSet<Edge>::Element *E = edges.front(); E; E = E->next()) {
			const Edge &e = E->get();
			Vector2 seg[2] = {
				points[e.points[0]].pos,
				points[e.points[1]].pos
			};

			Vector2 closest = Geometry2D::get_closest_point_to_segment(to, seg);
			float d = to.distance_squared_to(closest);

			if (d < closest_dist) {
				ignore_to_edge = E->get();
				closest_dist = d;
				closest_point = closest;
			}
		}

		to = closest_point;
	};

	//test direct connection
	{
		bool can_see_eachother = true;

		for (RBSet<Edge>::Element *E = edges.front(); E; E = E->next()) {
			const Edge &e = E->get();
			if (e.points[0] == ignore_from_edge.points[0] && e.points[1] == ignore_from_edge.points[1]) {
				continue;
			}
			if (e.points[0] == ignore_to_edge.points[0] && e.points[1] == ignore_to_edge.points[1]) {
				continue;
			}

			Vector2 a = points[e.points[0]].pos;
			Vector2 b = points[e.points[1]].pos;

			if (Geometry2D::segment_intersects_segment(a, b, from, to, nullptr)) {
				can_see_eachother = false;
				break;
			}
		}

		if (can_see_eachother) {
			path.push_back(from);
			path.push_back(to);
			return path;
		}
	}

	//add to graph

	int aidx = points.size() - 2;
	int bidx = points.size() - 1;
	points.write[aidx].pos = from;
	points.write[bidx].pos = to;
	points.write[aidx].distance = 0;
	points.write[bidx].distance = 0;
	points.write[aidx].prev = -1;
	points.write[bidx].prev = -1;
	points.write[aidx].penalty = 0;
	points.write[bidx].penalty = 0;

	for (int i = 0; i < points.size() - 2; i++) {
		bool valid_a = true;
		bool valid_b = true;
		points.write[i].prev = -1;
		points.write[i].distance = 0;

		if (!_is_point_inside(from * 0.5 + points[i].pos * 0.5)) {
			valid_a = false;
		}

		if (!_is_point_inside(to * 0.5 + points[i].pos * 0.5)) {
			valid_b = false;
		}

		for (RBSet<Edge>::Element *E = edges.front(); E; E = E->next()) {
			const Edge &e = E->get();

			if (e.points[0] == i || e.points[1] == i) {
				continue;
			}

			Vector2 a = points[e.points[0]].pos;
			Vector2 b = points[e.points[1]].pos;

			if (valid_a) {
				if (e.points[0] != ignore_from_edge.points[1] &&
						e.points[1] != ignore_from_edge.points[1] &&
						e.points[0] != ignore_from_edge.points[0] &&
						e.points[1] != ignore_from_edge.points[0]) {
					if (Geometry2D::segment_intersects_segment(a, b, from, points[i].pos, nullptr)) {
						valid_a = false;
					}
				}
			}

			if (valid_b) {
				if (e.points[0] != ignore_to_edge.points[1] &&
						e.points[1] != ignore_to_edge.points[1] &&
						e.points[0] != ignore_to_edge.points[0] &&
						e.points[1] != ignore_to_edge.points[0]) {
					if (Geometry2D::segment_intersects_segment(a, b, to, points[i].pos, nullptr)) {
						valid_b = false;
					}
				}
			}

			if (!valid_a && !valid_b) {
				break;
			}
		}

		if (valid_a) {
			points.write[i].connections.insert(aidx);
			points.write[aidx].connections.insert(i);
		}

		if (valid_b) {
			points.write[i].connections.insert(bidx);
			points.write[bidx].connections.insert(i);
		}
	}
	//solve graph

	RBSet<int> open_list;

	points.write[aidx].distance = 0;
	points.write[aidx].prev = aidx;
	for (RBSet<int>::Element *E = points[aidx].connections.front(); E; E = E->next()) {
		open_list.insert(E->get());
		points.write[E->get()].distance = from.distance_to(points[E->get()].pos);
		points.write[E->get()].prev = aidx;
	}

	bool found_route = false;

	while (true) {
		if (open_list.size() == 0) {
			printf("open list empty\n");
			break;
		}
		//check open list

		int least_cost_point = -1;
		float least_cost = 1e30;

		//this could be faster (cache previous results)
		for (RBSet<int>::Element *E = open_list.front(); E; E = E->next()) {
			const Point &p = points[E->get()];
			float cost = p.distance;
			cost += p.pos.distance_to(to);
			cost += p.penalty;

			if (cost < least_cost) {
				least_cost_point = E->get();
				least_cost = cost;
			}
		}

		const Point &np = points[least_cost_point];
		//open the neighbours for search

		for (RBSet<int>::Element *E = np.connections.front(); E; E = E->next()) {
			Point &p = points.write[E->get()];
			float distance = np.pos.distance_to(p.pos) + np.distance;

			if (p.prev != -1) {
				//oh this was visited already, can we win the cost?

				if (p.distance > distance) {
					p.prev = least_cost_point; //reassign previous
					p.distance = distance;
				}
			} else {
				//add to open neighbours

				p.prev = least_cost_point;
				p.distance = distance;
				open_list.insert(E->get());

				if (E->get() == bidx) {
					//oh my reached end! stop algorithm
					found_route = true;
					break;
				}
			}
		}

		if (found_route) {
			break;
		}

		open_list.erase(least_cost_point);
	}

	if (found_route) {
		int at = bidx;
		path.push_back(points[at].pos);
		do {
			at = points[at].prev;
			path.push_back(points[at].pos);
		} while (at != aidx);

		path.reverse();
	}

	for (int i = 0; i < points.size() - 2; i++) {
		points.write[i].connections.erase(aidx);
		points.write[i].connections.erase(bidx);
		points.write[i].prev = -1;
		points.write[i].distance = 0;
	}

	points.write[aidx].connections.clear();
	points.write[aidx].prev = -1;
	points.write[aidx].distance = 0;
	points.write[bidx].connections.clear();
	points.write[bidx].prev = -1;
	points.write[bidx].distance = 0;

	return path;
}

void PolygonPathFinder::_set_data(const Dictionary &p_data) {
	ERR_FAIL_COND(!p_data.has("points"));
	ERR_FAIL_COND(!p_data.has("connections"));
	ERR_FAIL_COND(!p_data.has("segments"));
	ERR_FAIL_COND(!p_data.has("bounds"));

	Vector<Vector2> p = p_data["points"];
	Array c = p_data["connections"];

	ERR_FAIL_COND(c.size() != p.size());
	if (c.size()) {
		return;
	}

	int pc = p.size();
	points.resize(pc + 2);

	const Vector2 *pr = p.ptr();
	for (int i = 0; i < pc; i++) {
		points.write[i].pos = pr[i];
		Vector<int> con = c[i];
		const int *cr = con.ptr();
		int cc = con.size();
		for (int j = 0; j < cc; j++) {
			points.write[i].connections.insert(cr[j]);
		}
	}

	if (p_data.has("penalties")) {
		Vector<real_t> penalties = p_data["penalties"];
		if (penalties.size() == pc) {
			const real_t *pr2 = penalties.ptr();
			for (int i = 0; i < pc; i++) {
				points.write[i].penalty = pr2[i];
			}
		}
	}

	Vector<int> segs = p_data["segments"];
	int sc = segs.size();
	ERR_FAIL_COND(sc & 1);
	const int *sr = segs.ptr();
	for (int i = 0; i < sc; i += 2) {
		Edge e(sr[i], sr[i + 1]);
		edges.insert(e);
	}
	bounds = p_data["bounds"];
}

Dictionary PolygonPathFinder::_get_data() const {
	Dictionary d;
	Vector<Vector2> p;
	Vector<int> ind;
	Array connections;
	p.resize(MAX(0, points.size() - 2));
	connections.resize(MAX(0, points.size() - 2));
	ind.resize(edges.size() * 2);
	Vector<real_t> penalties;
	penalties.resize(MAX(0, points.size() - 2));
	{
		Vector2 *wp = p.ptrw();
		real_t *pw = penalties.ptrw();

		for (int i = 0; i < points.size() - 2; i++) {
			wp[i] = points[i].pos;
			pw[i] = points[i].penalty;
			Vector<int> c;
			c.resize(points[i].connections.size());
			{
				int *cw = c.ptrw();
				int idx = 0;
				for (RBSet<int>::Element *E = points[i].connections.front(); E; E = E->next()) {
					cw[idx++] = E->get();
				}
			}
			connections[i] = c;
		}
	}
	{
		int *iw = ind.ptrw();
		int idx = 0;
		for (RBSet<Edge>::Element *E = edges.front(); E; E = E->next()) {
			iw[idx++] = E->get().points[0];
			iw[idx++] = E->get().points[1];
		}
	}

	d["bounds"] = bounds;
	d["points"] = p;
	d["penalties"] = penalties;
	d["connections"] = connections;
	d["segments"] = ind;

	return d;
}

bool PolygonPathFinder::is_point_inside(const Vector2 &p_point) const {
	return _is_point_inside(p_point);
}

Vector2 PolygonPathFinder::get_closest_point(const Vector2 &p_point) const {
	float closest_dist = 1e20f;
	Vector2 closest_point;

	for (RBSet<Edge>::Element *E = edges.front(); E; E = E->next()) {
		const Edge &e = E->get();
		Vector2 seg[2] = {
			points[e.points[0]].pos,
			points[e.points[1]].pos
		};

		Vector2 closest = Geometry2D::get_closest_point_to_segment(p_point, seg);
		float d = p_point.distance_squared_to(closest);

		if (d < closest_dist) {
			closest_dist = d;
			closest_point = closest;
		}
	}

	ERR_FAIL_COND_V(Math::is_equal_approx(closest_dist, 1e20f), Vector2());

	return closest_point;
}

Vector<Vector2> PolygonPathFinder::get_intersections(const Vector2 &p_from, const Vector2 &p_to) const {
	Vector<Vector2> inters;

	for (RBSet<Edge>::Element *E = edges.front(); E; E = E->next()) {
		Vector2 a = points[E->get().points[0]].pos;
		Vector2 b = points[E->get().points[1]].pos;

		Vector2 res;
		if (Geometry2D::segment_intersects_segment(a, b, p_from, p_to, &res)) {
			inters.push_back(res);
		}
	}

	return inters;
}

Rect2 PolygonPathFinder::get_bounds() const {
	return bounds;
}

void PolygonPathFinder::set_point_penalty(int p_point, float p_penalty) {
	ERR_FAIL_INDEX(p_point, points.size() - 2);
	points.write[p_point].penalty = p_penalty;
}

float PolygonPathFinder::get_point_penalty(int p_point) const {
	ERR_FAIL_INDEX_V(p_point, points.size() - 2, 0);
	return points[p_point].penalty;
}

void PolygonPathFinder::_bind_methods() {
	ClassDB::bind_method(D_METHOD("setup", "points", "connections"), &PolygonPathFinder::setup);
	ClassDB::bind_method(D_METHOD("find_path", "from", "to"), &PolygonPathFinder::find_path);
	ClassDB::bind_method(D_METHOD("get_intersections", "from", "to"), &PolygonPathFinder::get_intersections);
	ClassDB::bind_method(D_METHOD("get_closest_point", "point"), &PolygonPathFinder::get_closest_point);
	ClassDB::bind_method(D_METHOD("is_point_inside", "point"), &PolygonPathFinder::is_point_inside);
	ClassDB::bind_method(D_METHOD("set_point_penalty", "idx", "penalty"), &PolygonPathFinder::set_point_penalty);
	ClassDB::bind_method(D_METHOD("get_point_penalty", "idx"), &PolygonPathFinder::get_point_penalty);

	ClassDB::bind_method(D_METHOD("get_bounds"), &PolygonPathFinder::get_bounds);
	ClassDB::bind_method(D_METHOD("_set_data"), &PolygonPathFinder::_set_data);
	ClassDB::bind_method(D_METHOD("_get_data"), &PolygonPathFinder::_get_data);

	ADD_PROPERTY(PropertyInfo(Variant::DICTIONARY, "data", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "_set_data", "_get_data");
}

PolygonPathFinder::PolygonPathFinder() {
}