- Integrated NavigationServer and Navigation2DServer.

- Added Navigation Agents and Obstacles.
- Integrated Collision Avoidance.

This work has been kindly sponsored by IMVU.

10 files changed, 843 insertions, 844 deletions

diff --git a/scene/2d/navigation_2d.cpp b/scene/2d/navigation_2d.cpp
index 9209cc5074..de01d97ad9 100644
--- a/scene/2d/navigation_2d.cpp
+++ b/scene/2d/navigation_2d.cpp
@@ -29,710 +29,52 @@
 /*************************************************************************/
 
 #include "navigation_2d.h"
+#include "servers/navigation_2d_server.h"
 
-#define USE_ENTRY_POINT
-
-void Navigation2D::_navpoly_link(int p_id) {
-
-	ERR_FAIL_COND(!navpoly_map.has(p_id));
-	NavMesh &nm = navpoly_map[p_id];
-	ERR_FAIL_COND(nm.linked);
-
-	PoolVector<Vector2> vertices = nm.navpoly->get_vertices();
-	int len = vertices.size();
-	if (len == 0)
-		return;
-
-	PoolVector<Vector2>::Read r = vertices.read();
-
-	for (int i = 0; i < nm.navpoly->get_polygon_count(); i++) {
-
-		//build
-
-		List<Polygon>::Element *P = nm.polygons.push_back(Polygon());
-		Polygon &p = P->get();
-		p.owner = &nm;
-
-		Vector<int> poly = nm.navpoly->get_polygon(i);
-		int plen = poly.size();
-		const int *indices = poly.ptr();
-		bool valid = true;
-		p.edges.resize(plen);
-
-		Vector2 center;
-		float sum = 0;
-
-		for (int j = 0; j < plen; j++) {
-
-			int idx = indices[j];
-			if (idx < 0 || idx >= len) {
-				valid = false;
-				break;
-			}
-
-			Polygon::Edge e;
-			Vector2 ep = nm.xform.xform(r[idx]);
-			center += ep;
-			e.point = _get_point(ep);
-			p.edges.write[j] = e;
-
-			int idxn = indices[(j + 1) % plen];
-			if (idxn < 0 || idxn >= len) {
-				valid = false;
-				break;
-			}
-
-			Vector2 epn = nm.xform.xform(r[idxn]);
-
-			sum += (epn.x - ep.x) * (epn.y + ep.y);
-		}
-
-		p.clockwise = sum > 0;
-
-		if (!valid) {
-			nm.polygons.pop_back();
-			ERR_CONTINUE(!valid);
-		}
-
-		p.center = center / plen;
-
-		//connect
-
-		for (int j = 0; j < plen; j++) {
-
-			int next = (j + 1) % plen;
-			EdgeKey ek(p.edges[j].point, p.edges[next].point);
-
-			Map<EdgeKey, Connection>::Element *C = connections.find(ek);
-			if (!C) {
+void Navigation2D::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("get_rid"), &Navigation2D::get_rid);
 
-				Connection c;
-				c.A = &p;
-				c.A_edge = j;
-				c.B = NULL;
-				c.B_edge = -1;
-				connections[ek] = c;
-			} else {
+	ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation2D::get_simple_path, DEFVAL(true));
 
-				if (C->get().B != NULL) {
-					ConnectionPending pending;
-					pending.polygon = &p;
-					pending.edge = j;
-					p.edges.write[j].P = C->get().pending.push_back(pending);
-					continue;
-				}
+	ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &Navigation2D::set_cell_size);
+	ClassDB::bind_method(D_METHOD("get_cell_size"), &Navigation2D::get_cell_size);
 
-				C->get().B = &p;
-				C->get().B_edge = j;
-				C->get().A->edges.write[C->get().A_edge].C = &p;
-				C->get().A->edges.write[C->get().A_edge].C_edge = j;
-				p.edges.write[j].C = C->get().A;
-				p.edges.write[j].C_edge = C->get().A_edge;
-				//connection successful.
-			}
-		}
-	}
+	ClassDB::bind_method(D_METHOD("set_edge_connection_margin", "margin"), &Navigation2D::set_edge_connection_margin);
+	ClassDB::bind_method(D_METHOD("get_edge_connection_margin"), &Navigation2D::get_edge_connection_margin);
 
-	nm.linked = true;
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "cell_size"), "set_cell_size", "get_cell_size");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "edge_connection_margin"), "set_edge_connection_margin", "get_edge_connection_margin");
 }
 
-void Navigation2D::_navpoly_unlink(int p_id) {
-
-	ERR_FAIL_COND(!navpoly_map.has(p_id));
-	NavMesh &nm = navpoly_map[p_id];
-	ERR_FAIL_COND(!nm.linked);
-
-	for (List<Polygon>::Element *E = nm.polygons.front(); E; E = E->next()) {
-
-		Polygon &p = E->get();
-
-		int ec = p.edges.size();
-		Polygon::Edge *edges = p.edges.ptrw();
-
-		for (int i = 0; i < ec; i++) {
-			int next = (i + 1) % ec;
-
-			EdgeKey ek(edges[i].point, edges[next].point);
-			Map<EdgeKey, Connection>::Element *C = connections.find(ek);
-			ERR_CONTINUE(!C);
-
-			if (edges[i].P) {
-				C->get().pending.erase(edges[i].P);
-				edges[i].P = NULL;
-
-			} else if (C->get().B) {
-				//disconnect
+void Navigation2D::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_READY: {
+			Navigation2DServer::get_singleton()->map_set_active(map, true);
+		} break;
+		case NOTIFICATION_EXIT_TREE: {
 
-				C->get().B->edges.write[C->get().B_edge].C = NULL;
-				C->get().B->edges.write[C->get().B_edge].C_edge = -1;
-				C->get().A->edges.write[C->get().A_edge].C = NULL;
-				C->get().A->edges.write[C->get().A_edge].C_edge = -1;
-
-				if (C->get().A == &E->get()) {
-
-					C->get().A = C->get().B;
-					C->get().A_edge = C->get().B_edge;
-				}
-				C->get().B = NULL;
-				C->get().B_edge = -1;
-
-				if (C->get().pending.size()) {
-					//reconnect if something is pending
-					ConnectionPending cp = C->get().pending.front()->get();
-					C->get().pending.pop_front();
-
-					C->get().B = cp.polygon;
-					C->get().B_edge = cp.edge;
-					C->get().A->edges.write[C->get().A_edge].C = cp.polygon;
-					C->get().A->edges.write[C->get().A_edge].C_edge = cp.edge;
-					cp.polygon->edges.write[cp.edge].C = C->get().A;
-					cp.polygon->edges.write[cp.edge].C_edge = C->get().A_edge;
-					cp.polygon->edges.write[cp.edge].P = NULL;
-				}
-
-			} else {
-				connections.erase(C);
-				//erase
-			}
-		}
+			Navigation2DServer::get_singleton()->map_set_active(map, false);
+		} break;
 	}
-
-	nm.polygons.clear();
-
-	nm.linked = false;
-}
-
-int Navigation2D::navpoly_add(const Ref<NavigationPolygon> &p_mesh, const Transform2D &p_xform, Object *p_owner) {
-
-	int id = last_id++;
-	NavMesh nm;
-	nm.linked = false;
-	nm.navpoly = p_mesh;
-	nm.xform = p_xform;
-	nm.owner = p_owner;
-	navpoly_map[id] = nm;
-
-	_navpoly_link(id);
-
-	return id;
 }
 
-void Navigation2D::navpoly_set_transform(int p_id, const Transform2D &p_xform) {
-
-	ERR_FAIL_COND(!navpoly_map.has(p_id));
-	NavMesh &nm = navpoly_map[p_id];
-	if (nm.xform == p_xform)
-		return; //bleh
-	_navpoly_unlink(p_id);
-	nm.xform = p_xform;
-	_navpoly_link(p_id);
+void Navigation2D::set_cell_size(float p_cell_size) {
+	cell_size = p_cell_size;
+	Navigation2DServer::get_singleton()->map_set_cell_size(map, cell_size);
 }
-void Navigation2D::navpoly_remove(int p_id) {
 
-	ERR_FAIL_COND(!navpoly_map.has(p_id));
-	_navpoly_unlink(p_id);
-	navpoly_map.erase(p_id);
+void Navigation2D::set_edge_connection_margin(float p_edge_connection_margin) {
+	edge_connection_margin = p_edge_connection_margin;
+	Navigation2DServer::get_singleton()->map_set_edge_connection_margin(map, edge_connection_margin);
 }
 
 Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize) {
-
-	Polygon *begin_poly = NULL;
-	Polygon *end_poly = NULL;
-	Vector2 begin_point;
-	Vector2 end_point;
-	float begin_d = 1e20;
-	float end_d = 1e20;
-
-	//look for point inside triangle
-
-	for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
-		if (!E->get().linked)
-			continue;
-		for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
-			Polygon &p = F->get();
-			if (begin_d || end_d) {
-				for (int i = 2; i < p.edges.size(); i++) {
-
-					if (begin_d > 0) {
-
-						if (Geometry::is_point_in_triangle(p_start, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
-							begin_poly = &p;
-							begin_point = p_start;
-							begin_d = 0;
-							if (end_d == 0)
-								break;
-						}
-					}
-
-					if (end_d > 0) {
-
-						if (Geometry::is_point_in_triangle(p_end, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
-							end_poly = &p;
-							end_point = p_end;
-							end_d = 0;
-							if (begin_d == 0)
-								break;
-						}
-					}
-				}
-			}
-
-			p.prev_edge = -1;
-		}
-	}
-
-	//start or end not inside triangle.. look for closest segment :|
-	if (begin_d || end_d) {
-		for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
-			if (!E->get().linked)
-				continue;
-			for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
-				Polygon &p = F->get();
-				int es = p.edges.size();
-				for (int i = 0; i < es; i++) {
-
-					Vector2 edge[2] = {
-						_get_vertex(p.edges[i].point),
-						_get_vertex(p.edges[(i + 1) % es].point)
-					};
-
-					if (begin_d > 0) {
-						Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_start, edge);
-						float d = spoint.distance_to(p_start);
-						if (d < begin_d) {
-							begin_poly = &p;
-							begin_point = spoint;
-							begin_d = d;
-						}
-					}
-
-					if (end_d > 0) {
-						Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_end, edge);
-						float d = spoint.distance_to(p_end);
-						if (d < end_d) {
-							end_poly = &p;
-							end_point = spoint;
-							end_d = d;
-						}
-					}
-				}
-			}
-		}
-	}
-
-	if (!begin_poly || !end_poly) {
-
-		return Vector<Vector2>(); //no path
-	}
-
-	if (begin_poly == end_poly) {
-
-		Vector<Vector2> path;
-		path.resize(2);
-		path.write[0] = begin_point;
-		path.write[1] = end_point;
-		return path;
-	}
-
-	bool found_route = false;
-
-	List<Polygon *> open_list;
-
-	begin_poly->entry = p_start;
-
-	for (int i = 0; i < begin_poly->edges.size(); i++) {
-
-		if (begin_poly->edges[i].C) {
-
-			begin_poly->edges[i].C->prev_edge = begin_poly->edges[i].C_edge;
-#ifdef USE_ENTRY_POINT
-			Vector2 edge[2] = {
-				_get_vertex(begin_poly->edges[i].point),
-				_get_vertex(begin_poly->edges[(i + 1) % begin_poly->edges.size()].point)
-			};
-
-			Vector2 entry = Geometry::get_closest_point_to_segment_2d(begin_poly->entry, edge);
-			begin_poly->edges[i].C->distance = begin_poly->entry.distance_to(entry);
-			begin_poly->edges[i].C->entry = entry;
-#else
-			begin_poly->edges[i].C->distance = begin_poly->center.distance_to(begin_poly->edges[i].C->center);
-#endif
-			open_list.push_back(begin_poly->edges[i].C);
-
-			if (begin_poly->edges[i].C == end_poly) {
-				found_route = true;
-			}
-		}
-	}
-
-	while (!found_route) {
-
-		if (open_list.size() == 0) {
-			break;
-		}
-		//check open list
-
-		List<Polygon *>::Element *least_cost_poly = NULL;
-		float least_cost = 1e30;
-
-		//this could be faster (cache previous results)
-		for (List<Polygon *>::Element *E = open_list.front(); E; E = E->next()) {
-
-			Polygon *p = E->get();
-
-			float cost = p->distance;
-
-#ifdef USE_ENTRY_POINT
-			int es = p->edges.size();
-
-			float shortest_distance = 1e30;
-
-			for (int i = 0; i < es; i++) {
-				Polygon::Edge &e = p->edges.write[i];
-
-				if (!e.C)
-					continue;
-
-				Vector2 edge[2] = {
-					_get_vertex(p->edges[i].point),
-					_get_vertex(p->edges[(i + 1) % es].point)
-				};
-
-				Vector2 edge_point = Geometry::get_closest_point_to_segment_2d(p->entry, edge);
-				float dist = p->entry.distance_to(edge_point);
-				if (dist < shortest_distance)
-					shortest_distance = dist;
-			}
-
-			cost += shortest_distance;
-#else
-			cost += p->center.distance_to(end_point);
-#endif
-			if (cost < least_cost) {
-				least_cost_poly = E;
-				least_cost = cost;
-			}
-		}
-
-		Polygon *p = least_cost_poly->get();
-		//open the neighbours for search
-		int es = p->edges.size();
-
-		for (int i = 0; i < es; i++) {
-
-			Polygon::Edge &e = p->edges.write[i];
-
-			if (!e.C)
-				continue;
-
-#ifdef USE_ENTRY_POINT
-			Vector2 edge[2] = {
-				_get_vertex(p->edges[i].point),
-				_get_vertex(p->edges[(i + 1) % es].point)
-			};
-
-			Vector2 edge_entry = Geometry::get_closest_point_to_segment_2d(p->entry, edge);
-			float distance = p->entry.distance_to(edge_entry) + p->distance;
-
-#else
-
-			float distance = p->center.distance_to(e.C->center) + p->distance;
-
-#endif
-
-			if (e.C->prev_edge != -1) {
-				//oh this was visited already, can we win the cost?
-
-				if (e.C->distance > distance) {
-
-					e.C->prev_edge = e.C_edge;
-					e.C->distance = distance;
-#ifdef USE_ENTRY_POINT
-					e.C->entry = edge_entry;
-#endif
-				}
-			} else {
-				//add to open neighbours
-
-				e.C->prev_edge = e.C_edge;
-				e.C->distance = distance;
-#ifdef USE_ENTRY_POINT
-				e.C->entry = edge_entry;
-#endif
-
-				open_list.push_back(e.C);
-
-				if (e.C == end_poly) {
-					//oh my reached end! stop algorithm
-					found_route = true;
-					break;
-				}
-			}
-		}
-
-		if (found_route)
-			break;
-
-		open_list.erase(least_cost_poly);
-	}
-
-	if (found_route) {
-
-		Vector<Vector2> path;
-
-		if (p_optimize) {
-			//string pulling
-
-			Vector2 apex_point = end_point;
-			Vector2 portal_left = apex_point;
-			Vector2 portal_right = apex_point;
-			Polygon *left_poly = end_poly;
-			Polygon *right_poly = end_poly;
-			Polygon *p = end_poly;
-
-			while (p) {
-
-				Vector2 left;
-				Vector2 right;
-
-//#define CLOCK_TANGENT(m_a,m_b,m_c) ( ((m_a)-(m_c)).cross((m_a)-(m_b)) )
-#define CLOCK_TANGENT(m_a, m_b, m_c) ((((m_a).x - (m_c).x) * ((m_b).y - (m_c).y) - ((m_b).x - (m_c).x) * ((m_a).y - (m_c).y)))
-
-				if (p == begin_poly) {
-					left = begin_point;
-					right = begin_point;
-				} else {
-					int prev = p->prev_edge;
-					int prev_n = (p->prev_edge + 1) % p->edges.size();
-					left = _get_vertex(p->edges[prev].point);
-					right = _get_vertex(p->edges[prev_n].point);
-
-					if (p->clockwise) {
-						SWAP(left, right);
-					}
-					/*if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5) < 0){
-						SWAP(left,right);
-					}*/
-				}
-
-				bool skip = false;
-
-				/*
-				print_line("-----\nAPEX: "+(apex_point-end_point));
-				print_line("LEFT:");
-				print_line("\tPortal: "+(portal_left-end_point));
-				print_line("\tPoint: "+(left-end_point));
-				print_line("\tLeft Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_left,left)));
-				print_line("\tLeft Distance: "+rtos(portal_left.distance_squared_to(apex_point)));
-				print_line("\tLeft Test: "+rtos(CLOCK_TANGENT(apex_point,left,portal_right)));
-				print_line("RIGHT:");
-				print_line("\tPortal: "+(portal_right-end_point));
-				print_line("\tPoint: "+(right-end_point));
-				print_line("\tRight Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_right,right)));
-				print_line("\tRight Distance: "+rtos(portal_right.distance_squared_to(apex_point)));
-				print_line("\tRight Test: "+rtos(CLOCK_TANGENT(apex_point,right,portal_left)));
-				*/
-
-				if (CLOCK_TANGENT(apex_point, portal_left, left) >= 0) {
-					//process
-					if (portal_left.is_equal_approx(apex_point) || CLOCK_TANGENT(apex_point, left, portal_right) > 0) {
-						left_poly = p;
-						portal_left = left;
-					} else {
-
-						apex_point = portal_right;
-						p = right_poly;
-						left_poly = p;
-						portal_left = apex_point;
-						portal_right = apex_point;
-						if (!path.size() || !path[path.size() - 1].is_equal_approx(apex_point))
-							path.push_back(apex_point);
-						skip = true;
-					}
-				}
-
-				if (!skip && CLOCK_TANGENT(apex_point, portal_right, right) <= 0) {
-					//process
-					if (portal_right.is_equal_approx(apex_point) || CLOCK_TANGENT(apex_point, right, portal_left) < 0) {
-						right_poly = p;
-						portal_right = right;
-					} else {
-
-						apex_point = portal_left;
-						p = left_poly;
-						right_poly = p;
-						portal_right = apex_point;
-						portal_left = apex_point;
-						if (!path.size() || !path[path.size() - 1].is_equal_approx(apex_point))
-							path.push_back(apex_point);
-					}
-				}
-
-				if (p != begin_poly)
-					p = p->edges[p->prev_edge].C;
-				else
-					p = NULL;
-			}
-
-		} else {
-			//midpoints
-			Polygon *p = end_poly;
-
-			while (true) {
-				int prev = p->prev_edge;
-				int prev_n = (p->prev_edge + 1) % p->edges.size();
-				Vector2 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point)) * 0.5;
-				path.push_back(point);
-				p = p->edges[prev].C;
-				if (p == begin_poly)
-					break;
-			}
-		}
-
-		if (!path.size() || !path[path.size() - 1].is_equal_approx(begin_point)) {
-			path.push_back(begin_point); // Add the begin point
-		} else {
-			path.write[path.size() - 1] = begin_point; // Replace first midpoint by the exact begin point
-		}
-
-		path.invert();
-
-		if (path.size() <= 1 || !path[path.size() - 1].is_equal_approx(end_point)) {
-			path.push_back(end_point); // Add the end point
-		} else {
-			path.write[path.size() - 1] = end_point; // Replace last midpoint by the exact end point
-		}
-
-		return path;
-	}
-
-	return Vector<Vector2>();
-}
-
-Vector2 Navigation2D::get_closest_point(const Vector2 &p_point) {
-
-	Vector2 closest_point = Vector2();
-	float closest_point_d = 1e20;
-
-	for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
-		if (!E->get().linked)
-			continue;
-		for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
-			Polygon &p = F->get();
-			for (int i = 2; i < p.edges.size(); i++) {
-
-				if (Geometry::is_point_in_triangle(p_point, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
-					return p_point; //inside triangle, nothing else to discuss
-				}
-			}
-		}
-	}
-
-	for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
-		if (!E->get().linked)
-			continue;
-		for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
-			Polygon &p = F->get();
-			int es = p.edges.size();
-			for (int i = 0; i < es; i++) {
-
-				Vector2 edge[2] = {
-					_get_vertex(p.edges[i].point),
-					_get_vertex(p.edges[(i + 1) % es].point)
-				};
-
-				Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_point, edge);
-				float d = spoint.distance_squared_to(p_point);
-				if (d < closest_point_d) {
-
-					closest_point = spoint;
-					closest_point_d = d;
-				}
-			}
-		}
-	}
-
-	return closest_point;
-}
-
-Object *Navigation2D::get_closest_point_owner(const Vector2 &p_point) {
-
-	Object *owner = NULL;
-	Vector2 closest_point = Vector2();
-	float closest_point_d = 1e20;
-
-	for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
-		if (!E->get().linked)
-			continue;
-		for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
-			Polygon &p = F->get();
-			for (int i = 2; i < p.edges.size(); i++) {
-
-				if (Geometry::is_point_in_triangle(p_point, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
-
-					return E->get().owner;
-				}
-			}
-		}
-	}
-
-	for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
-
-		if (!E->get().linked)
-			continue;
-		for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
-
-			Polygon &p = F->get();
-			int es = p.edges.size();
-			for (int i = 0; i < es; i++) {
-
-				Vector2 edge[2] = {
-					_get_vertex(p.edges[i].point),
-					_get_vertex(p.edges[(i + 1) % es].point)
-				};
-
-				Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_point, edge);
-				float d = spoint.distance_squared_to(p_point);
-				if (d < closest_point_d) {
-
-					closest_point = spoint;
-					closest_point_d = d;
-					owner = E->get().owner;
-				}
-			}
-		}
-	}
-
-	return owner;
-}
-
-void Navigation2D::_bind_methods() {
-
-	ClassDB::bind_method(D_METHOD("navpoly_add", "mesh", "xform", "owner"), &Navigation2D::navpoly_add, DEFVAL(Variant()));
-	ClassDB::bind_method(D_METHOD("navpoly_set_transform", "id", "xform"), &Navigation2D::navpoly_set_transform);
-	ClassDB::bind_method(D_METHOD("navpoly_remove", "id"), &Navigation2D::navpoly_remove);
-
-	ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation2D::get_simple_path, DEFVAL(true));
-	ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Navigation2D::get_closest_point);
-	ClassDB::bind_method(D_METHOD("get_closest_point_owner", "to_point"), &Navigation2D::get_closest_point_owner);
+	return Navigation2DServer::get_singleton()->map_get_path(map, p_start, p_end, p_optimize);
 }
 
 Navigation2D::Navigation2D() {
 
-	ERR_FAIL_COND(sizeof(Point) != 8);
-	cell_size = 1; // one pixel
-	last_id = 1;
+	map = Navigation2DServer::get_singleton()->map_create();
+	set_cell_size(10); // Ten pixels
+	set_edge_connection_margin(100);
 }
diff --git a/scene/2d/navigation_2d.h b/scene/2d/navigation_2d.h
index 41b9aea35a..08642a5489 100644
--- a/scene/2d/navigation_2d.h
+++ b/scene/2d/navigation_2d.h
@@ -38,135 +38,30 @@ class Navigation2D : public Node2D {
 
 	GDCLASS(Navigation2D, Node2D);
 
-	union Point {
+	RID map;
+	real_t cell_size;
+	real_t edge_connection_margin;
 
-		struct {
-			int64_t x : 32;
-			int64_t y : 32;
-		};
-
-		uint64_t key;
-		bool operator<(const Point &p_key) const { return key < p_key.key; }
-	};
-
-	struct EdgeKey {
-
-		Point a;
-		Point b;
-
-		bool operator<(const EdgeKey &p_key) const {
-			return (a.key == p_key.a.key) ? (b.key < p_key.b.key) : (a.key < p_key.a.key);
-		};
-
-		EdgeKey(const Point &p_a = Point(), const Point &p_b = Point()) :
-				a(p_a),
-				b(p_b) {
-			if (a.key > b.key) {
-				SWAP(a, b);
-			}
-		}
-	};
-
-	struct NavMesh;
-	struct Polygon;
-
-	struct ConnectionPending {
-
-		Polygon *polygon;
-		int edge;
-	};
-
-	struct Polygon {
-
-		struct Edge {
-			Point point;
-			Polygon *C; //connection
-			int C_edge;
-			List<ConnectionPending>::Element *P;
-			Edge() {
-				C = NULL;
-				C_edge = -1;
-				P = NULL;
-			}
-		};
-
-		Vector<Edge> edges;
-
-		Vector2 center;
-		Vector2 entry;
-
-		float distance;
-		int prev_edge;
-
-		bool clockwise;
-
-		NavMesh *owner;
-	};
-
-	struct Connection {
-
-		Polygon *A;
-		int A_edge;
-		Polygon *B;
-		int B_edge;
-
-		List<ConnectionPending> pending;
-
-		Connection() {
-			A = NULL;
-			B = NULL;
-			A_edge = -1;
-			B_edge = -1;
-		}
-	};
-
-	Map<EdgeKey, Connection> connections;
-
-	struct NavMesh {
-
-		Object *owner;
-		Transform2D xform;
-		bool linked;
-		Ref<NavigationPolygon> navpoly;
-		List<Polygon> polygons;
-	};
-
-	_FORCE_INLINE_ Point _get_point(const Vector2 &p_pos) const {
-
-		int x = int(Math::floor(p_pos.x / cell_size));
-		int y = int(Math::floor(p_pos.y / cell_size));
+protected:
+	static void _bind_methods();
+	void _notification(int p_what);
 
-		Point p;
-		p.key = 0;
-		p.x = x;
-		p.y = y;
-		return p;
+public:
+	RID get_rid() const {
+		return map;
 	}
 
-	_FORCE_INLINE_ Vector2 _get_vertex(const Point &p_point) const {
-
-		return Vector2(p_point.x, p_point.y) * cell_size;
+	void set_cell_size(float p_cell_size);
+	float get_cell_size() const {
+		return cell_size;
 	}
 
-	void _navpoly_link(int p_id);
-	void _navpoly_unlink(int p_id);
-
-	float cell_size;
-	Map<int, NavMesh> navpoly_map;
-	int last_id;
-
-protected:
-	static void _bind_methods();
-
-public:
-	//API should be as dynamic as possible
-	int navpoly_add(const Ref<NavigationPolygon> &p_mesh, const Transform2D &p_xform, Object *p_owner = NULL);
-	void navpoly_set_transform(int p_id, const Transform2D &p_xform);
-	void navpoly_remove(int p_id);
+	void set_edge_connection_margin(float p_edge_connection_margin);
+	float get_edge_connection_margin() const {
+		return edge_connection_margin;
+	}
 
 	Vector<Vector2> get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize = true);
-	Vector2 get_closest_point(const Vector2 &p_point);
-	Object *get_closest_point_owner(const Vector2 &p_point);
 
 	Navigation2D();
 };
diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp
new file mode 100644
index 0000000000..fe4d3b322f
--- /dev/null
+++ b/scene/2d/navigation_agent_2d.cpp
@@ -0,0 +1,341 @@
+/*************************************************************************/
+/*  navigation_agent_2d.cpp                                              */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2020 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 "navigation_agent_2d.h"
+
+#include "core/engine.h"
+#include "scene/2d/navigation_2d.h"
+#include "servers/navigation_2d_server.h"
+
+void NavigationAgent2D::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("set_target_desired_distance", "desired_distance"), &NavigationAgent2D::set_target_desired_distance);
+	ClassDB::bind_method(D_METHOD("get_target_desired_distance"), &NavigationAgent2D::get_target_desired_distance);
+
+	ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationAgent2D::set_radius);
+	ClassDB::bind_method(D_METHOD("get_radius"), &NavigationAgent2D::get_radius);
+
+	ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationAgent2D::set_navigation_node);
+	ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationAgent2D::get_navigation_node);
+
+	ClassDB::bind_method(D_METHOD("set_neighbor_dist", "neighbor_dist"), &NavigationAgent2D::set_neighbor_dist);
+	ClassDB::bind_method(D_METHOD("get_neighbor_dist"), &NavigationAgent2D::get_neighbor_dist);
+
+	ClassDB::bind_method(D_METHOD("set_max_neighbors", "max_neighbors"), &NavigationAgent2D::set_max_neighbors);
+	ClassDB::bind_method(D_METHOD("get_max_neighbors"), &NavigationAgent2D::get_max_neighbors);
+
+	ClassDB::bind_method(D_METHOD("set_time_horizon", "time_horizon"), &NavigationAgent2D::set_time_horizon);
+	ClassDB::bind_method(D_METHOD("get_time_horizon"), &NavigationAgent2D::get_time_horizon);
+
+	ClassDB::bind_method(D_METHOD("set_max_speed", "max_speed"), &NavigationAgent2D::set_max_speed);
+	ClassDB::bind_method(D_METHOD("get_max_speed"), &NavigationAgent2D::get_max_speed);
+
+	ClassDB::bind_method(D_METHOD("set_path_max_distance", "max_speed"), &NavigationAgent2D::set_path_max_distance);
+	ClassDB::bind_method(D_METHOD("get_path_max_distance"), &NavigationAgent2D::get_path_max_distance);
+
+	ClassDB::bind_method(D_METHOD("set_target_location", "location"), &NavigationAgent2D::set_target_location);
+	ClassDB::bind_method(D_METHOD("get_target_location"), &NavigationAgent2D::get_target_location);
+	ClassDB::bind_method(D_METHOD("get_next_location"), &NavigationAgent2D::get_next_location);
+	ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent2D::distance_to_target);
+	ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationAgent2D::set_velocity);
+	ClassDB::bind_method(D_METHOD("get_nav_path"), &NavigationAgent2D::get_nav_path);
+	ClassDB::bind_method(D_METHOD("get_nav_path_index"), &NavigationAgent2D::get_nav_path_index);
+	ClassDB::bind_method(D_METHOD("is_target_reached"), &NavigationAgent2D::is_target_reached);
+	ClassDB::bind_method(D_METHOD("is_target_reachable"), &NavigationAgent2D::is_target_reachable);
+	ClassDB::bind_method(D_METHOD("is_navigation_finished"), &NavigationAgent2D::is_navigation_finished);
+	ClassDB::bind_method(D_METHOD("get_final_location"), &NavigationAgent2D::get_final_location);
+
+	ClassDB::bind_method(D_METHOD("_avoidance_done", "new_velocity"), &NavigationAgent2D::_avoidance_done);
+
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "target_desired_distance", PROPERTY_HINT_RANGE, "0.1,100,0.01"), "set_target_desired_distance", "get_target_desired_distance");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius", PROPERTY_HINT_RANGE, "0.1,500,0.01"), "set_radius", "get_radius");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "neighbor_dist", PROPERTY_HINT_RANGE, "0.1,100000,0.01"), "set_neighbor_dist", "get_neighbor_dist");
+	ADD_PROPERTY(PropertyInfo(Variant::INT, "max_neighbors", PROPERTY_HINT_RANGE, "1,10000,1"), "set_max_neighbors", "get_max_neighbors");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "time_horizon", PROPERTY_HINT_RANGE, "0.1,10000,0.01"), "set_time_horizon", "get_time_horizon");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "max_speed", PROPERTY_HINT_RANGE, "0.1,100000,0.01"), "set_max_speed", "get_max_speed");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "path_max_distance", PROPERTY_HINT_RANGE, "10,100,1"), "set_path_max_distance", "get_path_max_distance");
+
+	ADD_SIGNAL(MethodInfo("path_changed"));
+	ADD_SIGNAL(MethodInfo("target_reached"));
+	ADD_SIGNAL(MethodInfo("navigation_finished"));
+	ADD_SIGNAL(MethodInfo("velocity_computed", PropertyInfo(Variant::VECTOR3, "safe_velocity")));
+}
+
+void NavigationAgent2D::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_READY: {
+
+			agent_parent = Object::cast_to<Node2D>(get_parent());
+
+			Navigation2DServer::get_singleton()->agent_set_callback(agent, this, "_avoidance_done");
+
+			// Search the navigation node and set it
+			{
+				Navigation2D *nav = NULL;
+				Node *p = get_parent();
+				while (p != NULL) {
+					nav = Object::cast_to<Navigation2D>(p);
+					if (nav != NULL)
+						p = NULL;
+					else
+						p = p->get_parent();
+				}
+
+				set_navigation(nav);
+			}
+
+			set_physics_process_internal(true);
+		} break;
+		case NOTIFICATION_EXIT_TREE: {
+			agent_parent = NULL;
+			set_navigation(NULL);
+			set_physics_process_internal(false);
+		} break;
+		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+			if (agent_parent) {
+
+				Navigation2DServer::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().get_origin());
+				if (!target_reached) {
+					if (distance_to_target() < target_desired_distance) {
+						emit_signal("target_reached");
+						target_reached = true;
+					}
+				}
+			}
+		} break;
+	}
+}
+
+NavigationAgent2D::NavigationAgent2D() :
+		agent_parent(NULL),
+		navigation(NULL),
+		agent(RID()),
+		target_desired_distance(1.0),
+		path_max_distance(3.0),
+		velocity_submitted(false),
+		target_reached(false),
+		navigation_finished(true) {
+	agent = Navigation2DServer::get_singleton()->agent_create();
+	set_neighbor_dist(500.0);
+	set_max_neighbors(10);
+	set_time_horizon(20.0);
+	set_radius(10.0);
+	set_max_speed(200.0);
+}
+
+NavigationAgent2D::~NavigationAgent2D() {
+	Navigation2DServer::get_singleton()->free(agent);
+	agent = RID(); // Pointless
+}
+
+void NavigationAgent2D::set_navigation(Navigation2D *p_nav) {
+	if (navigation == p_nav)
+		return; // Pointless
+
+	navigation = p_nav;
+	Navigation2DServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
+}
+
+void NavigationAgent2D::set_navigation_node(Node *p_nav) {
+	Navigation2D *nav = Object::cast_to<Navigation2D>(p_nav);
+	ERR_FAIL_COND(nav == NULL);
+	set_navigation(nav);
+}
+
+Node *NavigationAgent2D::get_navigation_node() const {
+	return Object::cast_to<Node>(navigation);
+}
+
+void NavigationAgent2D::set_target_desired_distance(real_t p_dd) {
+	target_desired_distance = p_dd;
+}
+
+void NavigationAgent2D::set_radius(real_t p_radius) {
+	radius = p_radius;
+	Navigation2DServer::get_singleton()->agent_set_radius(agent, radius);
+}
+
+void NavigationAgent2D::set_neighbor_dist(real_t p_dist) {
+	neighbor_dist = p_dist;
+	Navigation2DServer::get_singleton()->agent_set_neighbor_dist(agent, neighbor_dist);
+}
+
+void NavigationAgent2D::set_max_neighbors(int p_count) {
+	max_neighbors = p_count;
+	Navigation2DServer::get_singleton()->agent_set_max_neighbors(agent, max_neighbors);
+}
+
+void NavigationAgent2D::set_time_horizon(real_t p_time) {
+	time_horizon = p_time;
+	Navigation2DServer::get_singleton()->agent_set_time_horizon(agent, time_horizon);
+}
+
+void NavigationAgent2D::set_max_speed(real_t p_max_speed) {
+	max_speed = p_max_speed;
+	Navigation2DServer::get_singleton()->agent_set_max_speed(agent, max_speed);
+}
+
+void NavigationAgent2D::set_path_max_distance(real_t p_pmd) {
+	path_max_distance = p_pmd;
+}
+
+real_t NavigationAgent2D::get_path_max_distance() {
+	return path_max_distance;
+}
+
+void NavigationAgent2D::set_target_location(Vector2 p_location) {
+	target_location = p_location;
+	navigation_path.clear();
+	target_reached = false;
+	navigation_finished = false;
+}
+
+Vector2 NavigationAgent2D::get_target_location() const {
+	return target_location;
+}
+
+Vector2 NavigationAgent2D::get_next_location() {
+	update_navigation();
+	if (navigation_path.size() == 0) {
+		ERR_FAIL_COND_V(agent_parent == NULL, Vector2());
+		return agent_parent->get_global_transform().get_origin();
+	} else {
+		return navigation_path[nav_path_index];
+	}
+}
+
+real_t NavigationAgent2D::distance_to_target() const {
+	ERR_FAIL_COND_V(agent_parent == NULL, 0.0);
+	return agent_parent->get_global_transform().get_origin().distance_to(target_location);
+}
+
+bool NavigationAgent2D::is_target_reached() const {
+	return target_reached;
+}
+
+bool NavigationAgent2D::is_target_reachable() {
+	return target_desired_distance >= get_final_location().distance_to(target_location);
+}
+
+bool NavigationAgent2D::is_navigation_finished() {
+	update_navigation();
+	return navigation_finished;
+}
+
+Vector2 NavigationAgent2D::get_final_location() {
+	update_navigation();
+	if (navigation_path.size() == 0) {
+		return Vector2();
+	}
+	return navigation_path[navigation_path.size() - 1];
+}
+
+void NavigationAgent2D::set_velocity(Vector2 p_velocity) {
+	target_velocity = p_velocity;
+	Navigation2DServer::get_singleton()->agent_set_target_velocity(agent, target_velocity);
+	Navigation2DServer::get_singleton()->agent_set_velocity(agent, prev_safe_velocity);
+	velocity_submitted = true;
+}
+
+void NavigationAgent2D::_avoidance_done(Vector3 p_new_velocity) {
+	const Vector2 velocity = Vector2(p_new_velocity.x, p_new_velocity.z);
+	prev_safe_velocity = velocity;
+
+	if (!velocity_submitted) {
+		target_velocity = Vector2();
+		return;
+	}
+	velocity_submitted = false;
+
+	emit_signal("velocity_computed", velocity);
+}
+
+String NavigationAgent2D::get_configuration_warning() const {
+	if (!Object::cast_to<Node2D>(get_parent())) {
+		return TTR("The NavigationAgent2D can be used only under a Node2D node");
+	}
+
+	return String();
+}
+
+void NavigationAgent2D::update_navigation() {
+
+	if (agent_parent == NULL) return;
+	if (navigation == NULL) return;
+	if (update_frame_id == Engine::get_singleton()->get_physics_frames()) return;
+
+	update_frame_id = Engine::get_singleton()->get_physics_frames();
+
+	Vector2 o = agent_parent->get_global_transform().get_origin();
+
+	bool reload_path = false;
+
+	if (Navigation2DServer::get_singleton()->agent_is_map_changed(agent)) {
+		reload_path = true;
+	} else if (navigation_path.size() == 0) {
+		reload_path = true;
+	} else {
+		// Check if too far from the navigation path
+		if (nav_path_index > 0) {
+			Vector2 segment[2];
+			segment[0] = navigation_path[nav_path_index - 1];
+			segment[1] = navigation_path[nav_path_index];
+			Vector2 p = Geometry::get_closest_point_to_segment_2d(o, segment);
+			if (o.distance_to(p) >= path_max_distance) {
+				// To faraway, reload path
+				reload_path = true;
+			}
+		}
+	}
+
+	if (reload_path) {
+		navigation_path = Navigation2DServer::get_singleton()->map_get_path(navigation->get_rid(), o, target_location, true);
+		navigation_finished = false;
+		nav_path_index = 0;
+		emit_signal("path_changed");
+	}
+
+	if (navigation_path.size() == 0)
+		return;
+
+	// Check if we can advance the navigation path
+	if (navigation_finished == false) {
+		// Advances to the next far away location.
+		while (o.distance_to(navigation_path[nav_path_index]) < target_desired_distance) {
+			nav_path_index += 1;
+			if (nav_path_index == navigation_path.size()) {
+				nav_path_index -= 1;
+				navigation_finished = true;
+				emit_signal("navigation_finished");
+				break;
+			}
+		}
+	}
+}
diff --git a/scene/2d/navigation_agent_2d.h b/scene/2d/navigation_agent_2d.h
new file mode 100644
index 0000000000..60527091ce
--- /dev/null
+++ b/scene/2d/navigation_agent_2d.h
@@ -0,0 +1,150 @@
+/*************************************************************************/
+/*  navigation_agent_2d.h                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2020 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.                */
+/*************************************************************************/
+
+#ifndef NAVIGATION_AGENT_2D_H
+#define NAVIGATION_AGENT_2D_H
+
+#include "core/vector.h"
+#include "scene/main/node.h"
+
+class Node2D;
+class Navigation2D;
+
+class NavigationAgent2D : public Node {
+	GDCLASS(NavigationAgent2D, Node);
+
+	Node2D *agent_parent;
+	Navigation2D *navigation;
+
+	RID agent;
+
+	real_t target_desired_distance;
+	real_t radius;
+	real_t neighbor_dist;
+	int max_neighbors;
+	real_t time_horizon;
+	real_t max_speed;
+
+	real_t path_max_distance;
+
+	Vector2 target_location;
+	Vector<Vector2> navigation_path;
+	int nav_path_index;
+	bool velocity_submitted;
+	Vector2 prev_safe_velocity;
+	/// The submitted target velocity
+	Vector2 target_velocity;
+	bool target_reached;
+	bool navigation_finished;
+	// No initialized on purpose
+	uint32_t update_frame_id;
+
+protected:
+	static void _bind_methods();
+	void _notification(int p_what);
+
+public:
+	NavigationAgent2D();
+	virtual ~NavigationAgent2D();
+
+	void set_navigation(Navigation2D *p_nav);
+	const Navigation2D *get_navigation() const {
+		return navigation;
+	}
+
+	void set_navigation_node(Node *p_nav);
+	Node *get_navigation_node() const;
+
+	RID get_rid() const {
+		return agent;
+	}
+
+	void set_target_desired_distance(real_t p_dd);
+	real_t get_target_desired_distance() const {
+		return target_desired_distance;
+	}
+
+	void set_radius(real_t p_radius);
+	real_t get_radius() const {
+		return radius;
+	}
+
+	void set_neighbor_dist(real_t p_dist);
+	real_t get_neighbor_dist() const {
+		return neighbor_dist;
+	}
+
+	void set_max_neighbors(int p_count);
+	int get_max_neighbors() const {
+		return max_neighbors;
+	}
+
+	void set_time_horizon(real_t p_time);
+	real_t get_time_horizon() const {
+		return time_horizon;
+	}
+
+	void set_max_speed(real_t p_max_speed);
+	real_t get_max_speed() const {
+		return max_speed;
+	}
+
+	void set_path_max_distance(real_t p_pmd);
+	real_t get_path_max_distance();
+
+	void set_target_location(Vector2 p_location);
+	Vector2 get_target_location() const;
+
+	Vector2 get_next_location();
+
+	Vector<Vector2> get_nav_path() const {
+		return navigation_path;
+	}
+
+	int get_nav_path_index() const {
+		return nav_path_index;
+	}
+
+	real_t distance_to_target() const;
+	bool is_target_reached() const;
+	bool is_target_reachable();
+	bool is_navigation_finished();
+	Vector2 get_final_location();
+
+	void set_velocity(Vector2 p_velocity);
+	void _avoidance_done(Vector3 p_new_velocity);
+
+	virtual String get_configuration_warning() const;
+
+private:
+	void update_navigation();
+};
+
+#endif
diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp
new file mode 100644
index 0000000000..0740b616b6
--- /dev/null
+++ b/scene/2d/navigation_obstacle_2d.cpp
@@ -0,0 +1,154 @@
+/*************************************************************************/
+/*  navigation_obstacle.cpp                                              */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2020 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 "navigation_obstacle_2d.h"
+
+#include "scene/2d/collision_shape_2d.h"
+#include "scene/2d/navigation_2d.h"
+#include "scene/2d/physics_body_2d.h"
+#include "servers/navigation_2d_server.h"
+
+void NavigationObstacle2D::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationObstacle2D::set_navigation_node);
+	ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationObstacle2D::get_navigation_node);
+}
+
+void NavigationObstacle2D::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_READY: {
+
+			update_agent_shape();
+
+			// Search the navigation node and set it
+			{
+				Navigation2D *nav = NULL;
+				Node *p = get_parent();
+				while (p != NULL) {
+					nav = Object::cast_to<Navigation2D>(p);
+					if (nav != NULL)
+						p = NULL;
+					else
+						p = p->get_parent();
+				}
+
+				set_navigation(nav);
+			}
+
+			set_physics_process_internal(true);
+		} break;
+		case NOTIFICATION_EXIT_TREE: {
+			set_navigation(NULL);
+			set_physics_process_internal(false);
+		} break;
+		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+			Node2D *node = Object::cast_to<Node2D>(get_parent());
+			if (node) {
+				Navigation2DServer::get_singleton()->agent_set_position(agent, node->get_global_transform().get_origin());
+			}
+
+		} break;
+	}
+}
+
+NavigationObstacle2D::NavigationObstacle2D() :
+		navigation(NULL),
+		agent(RID()) {
+	agent = Navigation2DServer::get_singleton()->agent_create();
+}
+
+NavigationObstacle2D::~NavigationObstacle2D() {
+	Navigation2DServer::get_singleton()->free(agent);
+	agent = RID(); // Pointless
+}
+
+void NavigationObstacle2D::set_navigation(Navigation2D *p_nav) {
+	if (navigation == p_nav)
+		return; // Pointless
+
+	navigation = p_nav;
+	Navigation2DServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
+}
+
+void NavigationObstacle2D::set_navigation_node(Node *p_nav) {
+	Navigation2D *nav = Object::cast_to<Navigation2D>(p_nav);
+	ERR_FAIL_COND(nav == NULL);
+	set_navigation(nav);
+}
+
+Node *NavigationObstacle2D::get_navigation_node() const {
+	return Object::cast_to<Node>(navigation);
+}
+
+String NavigationObstacle2D::get_configuration_warning() const {
+	if (!Object::cast_to<Node2D>(get_parent())) {
+		return TTR("The NavigationObstacle2D only serves to provide collision avoidance to a Node2D object.");
+	}
+
+	return String();
+}
+
+void NavigationObstacle2D::update_agent_shape() {
+	Node *node = get_parent();
+
+	// Estimate the radius of this physics body
+	real_t radius = 0.0;
+	for (int i(0); i < node->get_child_count(); i++) {
+		// For each collision shape
+		CollisionShape2D *cs = Object::cast_to<CollisionShape2D>(node->get_child(i));
+		if (cs) {
+			// Take the distance between the Body center to the shape center
+			real_t r = cs->get_transform().get_origin().length();
+			if (cs->get_shape().is_valid()) {
+				// and add the enclosing shape radius
+				r += cs->get_shape()->get_enclosing_radius();
+			}
+			Size2 s = cs->get_global_transform().get_scale();
+			r *= MAX(s.x, s.y);
+			// Takes the biggest radius
+			radius = MAX(radius, r);
+		}
+	}
+	Node2D *node_2d = Object::cast_to<Node2D>(node);
+	if (node_2d) {
+		Vector2 s = node_2d->get_global_transform().get_scale();
+		radius *= MAX(s.x, s.y);
+	}
+
+	if (radius == 0.0)
+		radius = 1.0; // Never a 0 radius
+
+	// Initialize the Agent as an object
+	Navigation2DServer::get_singleton()->agent_set_neighbor_dist(agent, 0.0);
+	Navigation2DServer::get_singleton()->agent_set_max_neighbors(agent, 0);
+	Navigation2DServer::get_singleton()->agent_set_time_horizon(agent, 0.0);
+	Navigation2DServer::get_singleton()->agent_set_radius(agent, radius);
+	Navigation2DServer::get_singleton()->agent_set_max_speed(agent, 0.0);
+}
diff --git a/scene/2d/navigation_obstacle_2d.h b/scene/2d/navigation_obstacle_2d.h
new file mode 100644
index 0000000000..f098255799
--- /dev/null
+++ b/scene/2d/navigation_obstacle_2d.h
@@ -0,0 +1,71 @@
+/*************************************************************************/
+/*  navigation_obstacle.h                                                */
+/*************************************************************************/
+/*                       This file is part of:                           */
+/*                           GODOT ENGINE                                */
+/*                      https://godotengine.org                          */
+/*************************************************************************/
+/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2020 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.                */
+/*************************************************************************/
+
+#ifndef NAVIGATION_OBSTACLE_2D_H
+#define NAVIGATION_OBSTACLE_2D_H
+
+#include "scene/main/node.h"
+
+class Navigation2D;
+
+class NavigationObstacle2D : public Node {
+	GDCLASS(NavigationObstacle2D, Node);
+
+	Navigation2D *navigation;
+
+	RID agent;
+
+protected:
+	static void _bind_methods();
+	void _notification(int p_what);
+
+public:
+	NavigationObstacle2D();
+	virtual ~NavigationObstacle2D();
+
+	void set_navigation(Navigation2D *p_nav);
+	const Navigation2D *get_navigation() const {
+		return navigation;
+	}
+
+	void set_navigation_node(Node *p_nav);
+	Node *get_navigation_node() const;
+
+	RID get_rid() const {
+		return agent;
+	}
+
+	virtual String get_configuration_warning() const;
+
+private:
+	void update_agent_shape();
+};
+
+#endif
diff --git a/scene/2d/navigation_polygon.cpp b/scene/2d/navigation_polygon.cpp
index 883fb37668..c40cdb4720 100644
--- a/scene/2d/navigation_polygon.cpp
+++ b/scene/2d/navigation_polygon.cpp
@@ -32,7 +32,9 @@
 
 #include "core/core_string_names.h"
 #include "core/engine.h"
+#include "core/os/mutex.h"
 #include "navigation_2d.h"
+#include "servers/navigation_2d_server.h"
 
 #include "thirdparty/misc/triangulator.h"
 
@@ -80,6 +82,9 @@ bool NavigationPolygon::_edit_is_selected_on_click(const Point2 &p_point, double
 
 void NavigationPolygon::set_vertices(const PoolVector<Vector2> &p_vertices) {
 
+	navmesh_generation->lock();
+	navmesh.unref();
+	navmesh_generation->unlock();
 	vertices = p_vertices;
 	rect_cache_dirty = true;
 }
@@ -91,6 +96,9 @@ PoolVector<Vector2> NavigationPolygon::get_vertices() const {
 
 void NavigationPolygon::_set_polygons(const Array &p_array) {
 
+	navmesh_generation->lock();
+	navmesh.unref();
+	navmesh_generation->unlock();
 	polygons.resize(p_array.size());
 	for (int i = 0; i < p_array.size(); i++) {
 		polygons.write[i].indices = p_array[i];
@@ -133,6 +141,9 @@ void NavigationPolygon::add_polygon(const Vector<int> &p_polygon) {
 	Polygon polygon;
 	polygon.indices = p_polygon;
 	polygons.push_back(polygon);
+	navmesh_generation->lock();
+	navmesh.unref();
+	navmesh_generation->unlock();
 }
 
 void NavigationPolygon::add_outline_at_index(const PoolVector<Vector2> &p_outline, int p_index) {
@@ -153,6 +164,34 @@ Vector<int> NavigationPolygon::get_polygon(int p_idx) {
 void NavigationPolygon::clear_polygons() {
 
 	polygons.clear();
+	navmesh_generation->lock();
+	navmesh.unref();
+	navmesh_generation->unlock();
+}
+
+Ref<NavigationMesh> NavigationPolygon::get_mesh() {
+	navmesh_generation->lock();
+	if (navmesh.is_null()) {
+		navmesh.instance();
+		PoolVector<Vector3> verts;
+		{
+			verts.resize(get_vertices().size());
+			PoolVector<Vector3>::Write w = verts.write();
+
+			PoolVector<Vector2>::Read r = get_vertices().read();
+
+			for (int i(0); i < get_vertices().size(); i++) {
+				w[i] = Vector3(r[i].x, 0.0, r[i].y);
+			}
+		}
+		navmesh->set_vertices(verts);
+
+		for (int i(0); i < get_polygon_count(); i++) {
+			navmesh->add_polygon(get_polygon(i));
+		}
+	}
+	navmesh_generation->unlock();
+	return navmesh;
 }
 
 void NavigationPolygon::add_outline(const PoolVector<Vector2> &p_outline) {
@@ -191,6 +230,9 @@ void NavigationPolygon::clear_outlines() {
 }
 void NavigationPolygon::make_polygons_from_outlines() {
 
+	navmesh_generation->lock();
+	navmesh.unref();
+	navmesh_generation->unlock();
 	List<TriangulatorPoly> in_poly, out_poly;
 
 	Vector2 outside_point(-1e10, -1e10);
@@ -320,7 +362,9 @@ void NavigationPolygon::_bind_methods() {
 }
 
 NavigationPolygon::NavigationPolygon() :
-		rect_cache_dirty(true) {
+		rect_cache_dirty(true),
+		navmesh_generation(NULL) {
+	navmesh_generation = Mutex::create();
 }
 
 void NavigationPolygonInstance::set_enabled(bool p_enabled) {
@@ -334,18 +378,12 @@ void NavigationPolygonInstance::set_enabled(bool p_enabled) {
 
 	if (!enabled) {
 
-		if (nav_id != -1) {
-			navigation->navpoly_remove(nav_id);
-			nav_id = -1;
-		}
+		Navigation2DServer::get_singleton()->region_set_map(region, RID());
 	} else {
 
 		if (navigation) {
 
-			if (navpoly.is_valid()) {
-
-				nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
-			}
+			Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
 		}
 	}
 
@@ -382,9 +420,9 @@ void NavigationPolygonInstance::_notification(int p_what) {
 				navigation = Object::cast_to<Navigation2D>(c);
 				if (navigation) {
 
-					if (enabled && navpoly.is_valid()) {
+					if (enabled) {
 
-						nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
+						Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
 					}
 					break;
 				}
@@ -395,19 +433,14 @@ void NavigationPolygonInstance::_notification(int p_what) {
 		} break;
 		case NOTIFICATION_TRANSFORM_CHANGED: {
 
-			if (navigation && nav_id != -1) {
-				navigation->navpoly_set_transform(nav_id, get_relative_transform_to_parent(navigation));
-			}
+			Navigation2DServer::get_singleton()->region_set_transform(region, get_global_transform());
 
 		} break;
 		case NOTIFICATION_EXIT_TREE: {
 
 			if (navigation) {
 
-				if (nav_id != -1) {
-					navigation->navpoly_remove(nav_id);
-					nav_id = -1;
-				}
+				Navigation2DServer::get_singleton()->region_set_map(region, RID());
 			}
 			navigation = NULL;
 		} break;
@@ -466,24 +499,18 @@ void NavigationPolygonInstance::set_navigation_polygon(const Ref<NavigationPolyg
 		return;
 	}
 
-	if (navigation && nav_id != -1) {
-		navigation->navpoly_remove(nav_id);
-		nav_id = -1;
-	}
-
 	if (navpoly.is_valid()) {
 		navpoly->disconnect(CoreStringNames::get_singleton()->changed, this, "_navpoly_changed");
 	}
+
 	navpoly = p_navpoly;
+	Navigation2DServer::get_singleton()->region_set_navpoly(region, p_navpoly);
+
 	if (navpoly.is_valid()) {
 		navpoly->connect(CoreStringNames::get_singleton()->changed, this, "_navpoly_changed");
 	}
 	_navpoly_changed();
 
-	if (navigation && navpoly.is_valid() && enabled) {
-		nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
-	}
-
 	_change_notify("navpoly");
 	update_configuration_warning();
 }
@@ -536,8 +563,13 @@ void NavigationPolygonInstance::_bind_methods() {
 
 NavigationPolygonInstance::NavigationPolygonInstance() {
 
-	navigation = NULL;
-	nav_id = -1;
 	enabled = true;
 	set_notify_transform(true);
+	region = Navigation2DServer::get_singleton()->region_create();
+
+	navigation = NULL;
+}
+
+NavigationPolygonInstance::~NavigationPolygonInstance() {
+	Navigation2DServer::get_singleton()->free(region);
 }
diff --git a/scene/2d/navigation_polygon.h b/scene/2d/navigation_polygon.h
index cbc1711a32..92cfc8f5e6 100644
--- a/scene/2d/navigation_polygon.h
+++ b/scene/2d/navigation_polygon.h
@@ -32,6 +32,9 @@
 #define NAVIGATION_POLYGON_H
 
 #include "scene/2d/node_2d.h"
+#include "scene/resources/navigation_mesh.h"
+
+class Mutex;
 
 class NavigationPolygon : public Resource {
 
@@ -47,6 +50,10 @@ class NavigationPolygon : public Resource {
 	mutable Rect2 item_rect;
 	mutable bool rect_cache_dirty;
 
+	Mutex *navmesh_generation;
+	// Navigation mesh
+	Ref<NavigationMesh> navmesh;
+
 protected:
 	static void _bind_methods();
 
@@ -81,6 +88,8 @@ public:
 	Vector<int> get_polygon(int p_idx);
 	void clear_polygons();
 
+	Ref<NavigationMesh> get_mesh();
+
 	NavigationPolygon();
 };
 
@@ -91,7 +100,7 @@ class NavigationPolygonInstance : public Node2D {
 	GDCLASS(NavigationPolygonInstance, Node2D);
 
 	bool enabled;
-	int nav_id;
+	RID region;
 	Navigation2D *navigation;
 	Ref<NavigationPolygon> navpoly;
 
@@ -116,6 +125,7 @@ public:
 	String get_configuration_warning() const;
 
 	NavigationPolygonInstance();
+	~NavigationPolygonInstance();
 };
 
 #endif // NAVIGATIONPOLYGON_H
diff --git a/scene/2d/tile_map.cpp b/scene/2d/tile_map.cpp
index b6db025d44..d8b880e571 100644
--- a/scene/2d/tile_map.cpp
+++ b/scene/2d/tile_map.cpp
@@ -35,6 +35,7 @@
 #include "core/method_bind_ext.gen.inc"
 #include "core/os/os.h"
 #include "scene/2d/area_2d.h"
+#include "servers/navigation_2d_server.h"
 #include "servers/physics_2d_server.h"
 
 int TileMap::_get_quadrant_size() const {
@@ -86,7 +87,7 @@ void TileMap::_notification(int p_what) {
 				if (navigation) {
 					for (Map<PosKey, Quadrant::NavPoly>::Element *F = q.navpoly_ids.front(); F; F = F->next()) {
 
-						navigation->navpoly_remove(F->get().id);
+						Navigation2DServer::get_singleton()->region_set_map(F->get().region, RID());
 					}
 					q.navpoly_ids.clear();
 				}
@@ -163,7 +164,7 @@ void TileMap::_update_quadrant_transform() {
 		if (navigation) {
 			for (Map<PosKey, Quadrant::NavPoly>::Element *F = q.navpoly_ids.front(); F; F = F->next()) {
 
-				navigation->navpoly_set_transform(F->get().id, nav_rel * F->get().xform);
+				Navigation2DServer::get_singleton()->region_set_transform(F->get().region, nav_rel * F->get().xform);
 			}
 		}
 
@@ -377,7 +378,7 @@ void TileMap::update_dirty_quadrants() {
 		if (navigation) {
 			for (Map<PosKey, Quadrant::NavPoly>::Element *E = q.navpoly_ids.front(); E; E = E->next()) {
 
-				navigation->navpoly_remove(E->get().id);
+				Navigation2DServer::get_singleton()->region_set_map(E->get().region, RID());
 			}
 			q.navpoly_ids.clear();
 		}
@@ -611,10 +612,13 @@ void TileMap::update_dirty_quadrants() {
 					xform.set_origin(offset.floor() + q.pos);
 					_fix_cell_transform(xform, c, npoly_ofs, s);
 
-					int pid = navigation->navpoly_add(navpoly, nav_rel * xform);
+					RID region = Navigation2DServer::get_singleton()->region_create();
+					Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
+					Navigation2DServer::get_singleton()->region_set_transform(region, nav_rel * xform);
+					Navigation2DServer::get_singleton()->region_set_navpoly(region, navpoly);
 
 					Quadrant::NavPoly np;
-					np.id = pid;
+					np.region = region;
 					np.xform = xform;
 					q.navpoly_ids[E->key()] = np;
 
@@ -809,7 +813,7 @@ void TileMap::_erase_quadrant(Map<PosKey, Quadrant>::Element *Q) {
 	if (navigation) {
 		for (Map<PosKey, Quadrant::NavPoly>::Element *E = q.navpoly_ids.front(); E; E = E->next()) {
 
-			navigation->navpoly_remove(E->get().id);
+			Navigation2DServer::get_singleton()->region_set_map(E->get().region, RID());
 		}
 		q.navpoly_ids.clear();
 	}
diff --git a/scene/2d/tile_map.h b/scene/2d/tile_map.h
index 0875d197eb..d5ef7fc818 100644
--- a/scene/2d/tile_map.h
+++ b/scene/2d/tile_map.h
@@ -139,7 +139,7 @@ private:
 		SelfList<Quadrant> dirty_list;
 
 		struct NavPoly {
-			int id;
+			RID region;
 			Transform2D xform;
 		};