1 files changed, 165 insertions, 10 deletions

diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index 3d71e66f80..b61119d8df 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -99,14 +99,22 @@ void AStar::remove_point(int p_id) {
 
 	Point *p = points[p_id];
 
-	Map<int, Point *>::Element *PE = points.front();
-	while (PE) {
-		for (Set<Point *>::Element *E = PE->get()->neighbours.front(); E; E = E->next()) {
-			Segment s(p_id, E->get()->id);
-			segments.erase(s);
-			E->get()->neighbours.erase(p);
-		}
-		PE = PE->next();
+	for (Set<Point *>::Element *E = p->neighbours.front(); E; E = E->next()) {
+
+		Segment s(p_id, E->get()->id);
+		segments.erase(s);
+
+		E->get()->neighbours.erase(p);
+		E->get()->unlinked_neighbours.erase(p);
+	}
+
+	for (Set<Point *>::Element *E = p->unlinked_neighbours.front(); E; E = E->next()) {
+
+		Segment s(p_id, E->get()->id);
+		segments.erase(s);
+
+		E->get()->neighbours.erase(p);
+		E->get()->unlinked_neighbours.erase(p);
 	}
 
 	memdelete(p);
@@ -125,6 +133,8 @@ void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) {
 
 	if (bidirectional)
 		b->neighbours.insert(a);
+	else
+		b->unlinked_neighbours.insert(a);
 
 	Segment s(p_id, p_with_id);
 	if (s.from == p_id) {
@@ -147,7 +157,9 @@ void AStar::disconnect_points(int p_id, int p_with_id) {
 	Point *a = points[p_id];
 	Point *b = points[p_with_id];
 	a->neighbours.erase(b);
+	a->unlinked_neighbours.erase(b);
 	b->neighbours.erase(a);
+	b->unlinked_neighbours.erase(a);
 }
 
 bool AStar::has_point(int p_id) const {
@@ -204,6 +216,8 @@ int AStar::get_closest_point(const Vector3 &p_point) const {
 
 	for (const Map<int, Point *>::Element *E = points.front(); E; E = E->next()) {
 
+		if (!E->get()->enabled)
+			continue; //Disabled points should not be considered
 		real_t d = p_point.distance_squared_to(E->get()->pos);
 		if (closest_id < 0 || d < closest_dist) {
 			closest_dist = d;
@@ -222,6 +236,10 @@ Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const {
 
 	for (const Set<Segment>::Element *E = segments.front(); E; E = E->next()) {
 
+		if (!(E->get().from_point->enabled && E->get().to_point->enabled)) {
+			continue;
+		}
+
 		Vector3 segment[2] = {
 			E->get().from_point->pos,
 			E->get().to_point->pos,
@@ -423,10 +441,16 @@ PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) {
 }
 
 void AStar::set_point_disabled(int p_id, bool p_disabled) {
+
+	ERR_FAIL_COND(!points.has(p_id));
+
 	points[p_id]->enabled = !p_disabled;
 }
 
 bool AStar::is_point_disabled(int p_id) const {
+
+	ERR_FAIL_COND_V(!points.has(p_id), false);
+
 	return !points[p_id]->enabled;
 }
 
@@ -440,13 +464,12 @@ void AStar::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar::set_point_weight_scale);
 	ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar::remove_point);
 	ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar::has_point);
+	ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar::get_point_connections);
 	ClassDB::bind_method(D_METHOD("get_points"), &AStar::get_points);
 
 	ClassDB::bind_method(D_METHOD("set_point_disabled", "id", "disabled"), &AStar::set_point_disabled, DEFVAL(true));
 	ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar::is_point_disabled);
 
-	ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar::get_point_connections);
-
 	ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar::connect_points, DEFVAL(true));
 	ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id"), &AStar::disconnect_points);
 	ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id"), &AStar::are_points_connected);
@@ -473,3 +496,135 @@ AStar::~AStar() {
 	pass = 1;
 	clear();
 }
+
+/////////////////////////////////////////////////////////////
+
+int AStar2D::get_available_point_id() const {
+	return astar.get_available_point_id();
+}
+
+void AStar2D::add_point(int p_id, const Vector2 &p_pos, real_t p_weight_scale) {
+	astar.add_point(p_id, Vector3(p_pos.x, p_pos.y, 0), p_weight_scale);
+}
+
+Vector2 AStar2D::get_point_position(int p_id) const {
+	Vector3 p = astar.get_point_position(p_id);
+	return Vector2(p.x, p.y);
+}
+
+void AStar2D::set_point_position(int p_id, const Vector2 &p_pos) {
+	astar.set_point_position(p_id, Vector3(p_pos.x, p_pos.y, 0));
+}
+
+real_t AStar2D::get_point_weight_scale(int p_id) const {
+	return astar.get_point_weight_scale(p_id);
+}
+
+void AStar2D::set_point_weight_scale(int p_id, real_t p_weight_scale) {
+	astar.set_point_weight_scale(p_id, p_weight_scale);
+}
+
+void AStar2D::remove_point(int p_id) {
+	astar.remove_point(p_id);
+}
+
+bool AStar2D::has_point(int p_id) const {
+	return astar.has_point(p_id);
+}
+
+PoolVector<int> AStar2D::get_point_connections(int p_id) {
+	return astar.get_point_connections(p_id);
+}
+
+Array AStar2D::get_points() {
+	return astar.get_points();
+}
+
+void AStar2D::set_point_disabled(int p_id, bool p_disabled) {
+	astar.set_point_disabled(p_id, p_disabled);
+}
+
+bool AStar2D::is_point_disabled(int p_id) const {
+	return astar.is_point_disabled(p_id);
+}
+
+void AStar2D::connect_points(int p_id, int p_with_id, bool p_bidirectional) {
+	astar.connect_points(p_id, p_with_id, p_bidirectional);
+}
+
+void AStar2D::disconnect_points(int p_id, int p_with_id) {
+	astar.disconnect_points(p_id, p_with_id);
+}
+
+bool AStar2D::are_points_connected(int p_id, int p_with_id) const {
+	return astar.are_points_connected(p_id, p_with_id);
+}
+
+void AStar2D::clear() {
+	astar.clear();
+}
+
+int AStar2D::get_closest_point(const Vector2 &p_point) const {
+	return astar.get_closest_point(Vector3(p_point.x, p_point.y, 0));
+}
+
+Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const {
+	Vector3 p = astar.get_closest_position_in_segment(Vector3(p_point.x, p_point.y, 0));
+	return Vector2(p.x, p.y);
+}
+
+PoolVector<Vector2> AStar2D::get_point_path(int p_from_id, int p_to_id) {
+
+	PoolVector3Array pv = astar.get_point_path(p_from_id, p_to_id);
+	int size = pv.size();
+	PoolVector2Array path;
+	path.resize(size);
+	{
+		PoolVector<Vector3>::Read r = pv.read();
+		PoolVector<Vector2>::Write w = path.write();
+		for (int i = 0; i < size; i++) {
+			Vector3 p = r[i];
+			w[i] = Vector2(p.x, p.y);
+		}
+	}
+	return path;
+}
+
+PoolVector<int> AStar2D::get_id_path(int p_from_id, int p_to_id) {
+	return astar.get_id_path(p_from_id, p_to_id);
+}
+
+void AStar2D::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar2D::get_available_point_id);
+	ClassDB::bind_method(D_METHOD("add_point", "id", "position", "weight_scale"), &AStar2D::add_point, DEFVAL(1.0));
+	ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStar2D::get_point_position);
+	ClassDB::bind_method(D_METHOD("set_point_position", "id", "position"), &AStar2D::set_point_position);
+	ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStar2D::get_point_weight_scale);
+	ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar2D::set_point_weight_scale);
+	ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar2D::remove_point);
+	ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar2D::has_point);
+	ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar2D::get_point_connections);
+	ClassDB::bind_method(D_METHOD("get_points"), &AStar2D::get_points);
+
+	ClassDB::bind_method(D_METHOD("set_point_disabled", "id", "disabled"), &AStar2D::set_point_disabled, DEFVAL(true));
+	ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar2D::is_point_disabled);
+
+	ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar2D::connect_points, DEFVAL(true));
+	ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id"), &AStar2D::disconnect_points);
+	ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id"), &AStar2D::are_points_connected);
+
+	ClassDB::bind_method(D_METHOD("clear"), &AStar2D::clear);
+
+	ClassDB::bind_method(D_METHOD("get_closest_point", "to_position"), &AStar2D::get_closest_point);
+	ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar2D::get_closest_position_in_segment);
+
+	ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar2D::get_point_path);
+	ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar2D::get_id_path);
+}
+
+AStar2D::AStar2D() {
+}
+
+AStar2D::~AStar2D() {
+}