diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index 73f190a3302..df77dd94948 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -399,7 +399,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
 	return found_route;
 }
 
-float AStar::_estimate_cost(int p_from_id, int p_to_id) {
+real_t AStar::_estimate_cost(int p_from_id, int p_to_id) {
 
 	if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_estimate_cost))
 		return get_script_instance()->call(SceneStringNames::get_singleton()->_estimate_cost, p_from_id, p_to_id);
@@ -415,7 +415,7 @@ float AStar::_estimate_cost(int p_from_id, int p_to_id) {
 	return from_point->pos.distance_to(to_point->pos);
 }
 
-float AStar::_compute_cost(int p_from_id, int p_to_id) {
+real_t AStar::_compute_cost(int p_from_id, int p_to_id) {
 
 	if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_compute_cost))
 		return get_script_instance()->call(SceneStringNames::get_singleton()->_compute_cost, p_from_id, p_to_id);
@@ -677,25 +677,195 @@ Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const {
 	return Vector2(p.x, p.y);
 }
 
+real_t AStar2D::_estimate_cost(int p_from_id, int p_to_id) {
+
+	if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_estimate_cost))
+		return get_script_instance()->call(SceneStringNames::get_singleton()->_estimate_cost, p_from_id, p_to_id);
+
+	AStar::Point *from_point;
+	bool from_exists = astar.points.lookup(p_from_id, from_point);
+	ERR_FAIL_COND_V(!from_exists, 0);
+
+	AStar::Point *to_point;
+	bool to_exists = astar.points.lookup(p_to_id, to_point);
+	ERR_FAIL_COND_V(!to_exists, 0);
+
+	return from_point->pos.distance_to(to_point->pos);
+}
+
+real_t AStar2D::_compute_cost(int p_from_id, int p_to_id) {
+
+	if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_compute_cost))
+		return get_script_instance()->call(SceneStringNames::get_singleton()->_compute_cost, p_from_id, p_to_id);
+
+	AStar::Point *from_point;
+	bool from_exists = astar.points.lookup(p_from_id, from_point);
+	ERR_FAIL_COND_V(!from_exists, 0);
+
+	AStar::Point *to_point;
+	bool to_exists = astar.points.lookup(p_to_id, to_point);
+	ERR_FAIL_COND_V(!to_exists, 0);
+
+	return from_point->pos.distance_to(to_point->pos);
+}
+
 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);
-		}
+	AStar::Point *a;
+	bool from_exists = astar.points.lookup(p_from_id, a);
+	ERR_FAIL_COND_V(!from_exists, PoolVector<Vector2>());
+
+	AStar::Point *b;
+	bool to_exists = astar.points.lookup(p_to_id, b);
+	ERR_FAIL_COND_V(!to_exists, PoolVector<Vector2>());
+
+	if (a == b) {
+		PoolVector<Vector2> ret;
+		ret.push_back(Vector2(a->pos.x, a->pos.y));
+		return ret;
 	}
+
+	AStar::Point *begin_point = a;
+	AStar::Point *end_point = b;
+
+	bool found_route = _solve(begin_point, end_point);
+	if (!found_route) return PoolVector<Vector2>();
+
+	AStar::Point *p = end_point;
+	int pc = 1; // Begin point
+	while (p != begin_point) {
+		pc++;
+		p = p->prev_point;
+	}
+
+	PoolVector<Vector2> path;
+	path.resize(pc);
+
+	{
+		PoolVector<Vector2>::Write w = path.write();
+
+		AStar::Point *p2 = end_point;
+		int idx = pc - 1;
+		while (p2 != begin_point) {
+			w[idx--] = Vector2(p2->pos.x, p2->pos.y);
+			p2 = p2->prev_point;
+		}
+
+		w[0] = Vector2(p2->pos.x, p2->pos.y); // Assign first
+	}
+
 	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);
+
+	AStar::Point *a;
+	bool from_exists = astar.points.lookup(p_from_id, a);
+	ERR_FAIL_COND_V(!from_exists, PoolVector<int>());
+
+	AStar::Point *b;
+	bool to_exists = astar.points.lookup(p_to_id, b);
+	ERR_FAIL_COND_V(!to_exists, PoolVector<int>());
+
+	if (a == b) {
+		PoolVector<int> ret;
+		ret.push_back(a->id);
+		return ret;
+	}
+
+	AStar::Point *begin_point = a;
+	AStar::Point *end_point = b;
+
+	bool found_route = _solve(begin_point, end_point);
+	if (!found_route) return PoolVector<int>();
+
+	AStar::Point *p = end_point;
+	int pc = 1; // Begin point
+	while (p != begin_point) {
+		pc++;
+		p = p->prev_point;
+	}
+
+	PoolVector<int> path;
+	path.resize(pc);
+
+	{
+		PoolVector<int>::Write w = path.write();
+
+		p = end_point;
+		int idx = pc - 1;
+		while (p != begin_point) {
+			w[idx--] = p->id;
+			p = p->prev_point;
+		}
+
+		w[0] = p->id; // Assign first
+	}
+
+	return path;
+}
+
+bool AStar2D::_solve(AStar::Point *begin_point, AStar::Point *end_point) {
+
+	astar.pass++;
+
+	if (!end_point->enabled) return false;
+
+	bool found_route = false;
+
+	Vector<AStar::Point *> open_list;
+	SortArray<AStar::Point *, AStar::SortPoints> sorter;
+
+	begin_point->g_score = 0;
+	begin_point->f_score = _estimate_cost(begin_point->id, end_point->id);
+	open_list.push_back(begin_point);
+
+	while (!open_list.empty()) {
+
+		AStar::Point *p = open_list[0]; // The currently processed point
+
+		if (p == end_point) {
+			found_route = true;
+			break;
+		}
+
+		sorter.pop_heap(0, open_list.size(), open_list.ptrw()); // Remove the current point from the open list
+		open_list.remove(open_list.size() - 1);
+		p->closed_pass = astar.pass; // Mark the point as closed
+
+		for (OAHashMap<int, AStar::Point *>::Iterator it = p->neighbours.iter(); it.valid; it = p->neighbours.next_iter(it)) {
+
+			AStar::Point *e = *(it.value); // The neighbour point
+
+			if (!e->enabled || e->closed_pass == astar.pass) {
+				continue;
+			}
+
+			real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * e->weight_scale;
+
+			bool new_point = false;
+
+			if (e->open_pass != astar.pass) { // The point wasn't inside the open list.
+				e->open_pass = astar.pass;
+				open_list.push_back(e);
+				new_point = true;
+			} else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous.
+				continue;
+			}
+
+			e->prev_point = p;
+			e->g_score = tentative_g_score;
+			e->f_score = e->g_score + _estimate_cost(e->id, end_point->id);
+
+			if (new_point) { // The position of the new points is already known.
+				sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptrw());
+			} else {
+				sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptrw());
+			}
+		}
+	}
+
+	return found_route;
 }
 
 void AStar2D::_bind_methods() {
@@ -728,6 +898,9 @@ void AStar2D::_bind_methods() {
 
 	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);
+
+	BIND_VMETHOD(MethodInfo(Variant::REAL, "_estimate_cost", PropertyInfo(Variant::INT, "from_id"), PropertyInfo(Variant::INT, "to_id")));
+	BIND_VMETHOD(MethodInfo(Variant::REAL, "_compute_cost", PropertyInfo(Variant::INT, "from_id"), PropertyInfo(Variant::INT, "to_id")));
 }
 
 AStar2D::AStar2D() {
diff --git a/core/math/a_star.h b/core/math/a_star.h
index 0b109769328..40a89c22ef3 100644
--- a/core/math/a_star.h
+++ b/core/math/a_star.h
@@ -43,6 +43,7 @@
 class AStar : public Reference {
 
 	GDCLASS(AStar, Reference);
+	friend class AStar2D;
 
 	struct Point {
 
@@ -124,8 +125,8 @@ class AStar : public Reference {
 protected:
 	static void _bind_methods();
 
-	virtual float _estimate_cost(int p_from_id, int p_to_id);
-	virtual float _compute_cost(int p_from_id, int p_to_id);
+	virtual real_t _estimate_cost(int p_from_id, int p_to_id);
+	virtual real_t _compute_cost(int p_from_id, int p_to_id);
 
 public:
 	int get_available_point_id() const;
@@ -166,9 +167,14 @@ class AStar2D : public Reference {
 	GDCLASS(AStar2D, Reference);
 	AStar astar;
 
+	bool _solve(AStar::Point *begin_point, AStar::Point *end_point);
+
 protected:
 	static void _bind_methods();
 
+	virtual real_t _estimate_cost(int p_from_id, int p_to_id);
+	virtual real_t _compute_cost(int p_from_id, int p_to_id);
+
 public:
 	int get_available_point_id() const;
 
diff --git a/doc/classes/AStar2D.xml b/doc/classes/AStar2D.xml
index 7a6ba746c72..a8a5b34f633 100644
--- a/doc/classes/AStar2D.xml
+++ b/doc/classes/AStar2D.xml
@@ -9,6 +9,30 @@
 	<tutorials>
 	</tutorials>
 	<methods>
+		<method name="_compute_cost" qualifiers="virtual">
+			<return type="float">
+			</return>
+			<argument index="0" name="from_id" type="int">
+			</argument>
+			<argument index="1" name="to_id" type="int">
+			</argument>
+			<description>
+				Called when computing the cost between two connected points.
+				Note that this function is hidden in the default [code]AStar2D[/code] class.
+			</description>
+		</method>
+		<method name="_estimate_cost" qualifiers="virtual">
+			<return type="float">
+			</return>
+			<argument index="0" name="from_id" type="int">
+			</argument>
+			<argument index="1" name="to_id" type="int">
+			</argument>
+			<description>
+				Called when estimating the cost between a point and the path's ending point.
+				Note that this function is hidden in the default [code]AStar2D[/code] class.
+			</description>
+		</method>
 		<method name="add_point">
 			<return type="void">
 			</return>