Merge pull request #34776 from AndreaCatania/nav_pr

Integrated the new `NavigationServer` and `NavigationServer2D`

59 files changed, 2158 insertions, 1876 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;
 		};
 
diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp
index 4a82fe0080..10b12f5c75 100644
--- a/scene/3d/navigation.cpp
+++ b/scene/3d/navigation.cpp
@@ -30,698 +30,76 @@
 
 #include "navigation.h"
 
-#define USE_ENTRY_POINT
+#include "servers/navigation_server.h"
 
-void Navigation::_navmesh_link(int p_id) {
-
-	ERR_FAIL_COND(!navmesh_map.has(p_id));
-	NavMesh &nm = navmesh_map[p_id];
-	ERR_FAIL_COND(nm.linked);
-	ERR_FAIL_COND(nm.navmesh.is_null());
-
-	PoolVector<Vector3> vertices = nm.navmesh->get_vertices();
-	int len = vertices.size();
-	if (len == 0)
-		return;
-
-	PoolVector<Vector3>::Read r = vertices.read();
-
-	for (int i = 0; i < nm.navmesh->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.navmesh->get_polygon(i);
-		int plen = poly.size();
-		const int *indices = poly.ptr();
-		bool valid = true;
-		p.edges.resize(plen);
-
-		Vector3 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;
-			Vector3 ep = nm.xform.xform(r[idx]);
-			center += ep;
-			e.point = _get_point(ep);
-			p.edges.write[j] = e;
-
-			if (j >= 2) {
-				Vector3 epa = nm.xform.xform(r[indices[j - 2]]);
-				Vector3 epb = nm.xform.xform(r[indices[j - 1]]);
-
-				sum += up.dot((epb - epa).cross(ep - epa));
-			}
-		}
-
-		p.clockwise = sum > 0;
-
-		if (!valid) {
-			nm.polygons.pop_back();
-			ERR_CONTINUE(!valid);
-		}
-
-		p.center = center;
-		if (plen != 0) {
-			p.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) {
-
-				Connection c;
-				c.A = &p;
-				c.A_edge = j;
-				c.B = NULL;
-				c.B_edge = -1;
-				connections[ek] = c;
-			} else {
-
-				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;
-				}
-
-				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.
-			}
-		}
-	}
-
-	nm.linked = true;
-}
-
-void Navigation::_navmesh_unlink(int p_id) {
-
-	ERR_FAIL_COND(!navmesh_map.has(p_id));
-	NavMesh &nm = navmesh_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
-
-				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
-			}
-		}
-	}
-
-	nm.polygons.clear();
-
-	nm.linked = false;
-}
-
-int Navigation::navmesh_add(const Ref<NavigationMesh> &p_mesh, const Transform &p_xform, Object *p_owner) {
-
-	int id = last_id++;
-	NavMesh nm;
-	nm.linked = false;
-	nm.navmesh = p_mesh;
-	nm.xform = p_xform;
-	nm.owner = p_owner;
-	navmesh_map[id] = nm;
-
-	_navmesh_link(id);
-
-	return id;
-}
-
-void Navigation::navmesh_set_transform(int p_id, const Transform &p_xform) {
-
-	ERR_FAIL_COND(!navmesh_map.has(p_id));
-	NavMesh &nm = navmesh_map[p_id];
-	if (nm.xform == p_xform)
-		return; //bleh
-	_navmesh_unlink(p_id);
-	nm.xform = p_xform;
-	_navmesh_link(p_id);
-}
-void Navigation::navmesh_remove(int p_id) {
+Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize) {
 
-	ERR_FAIL_COND_MSG(!navmesh_map.has(p_id), "Trying to remove nonexisting navmesh with id: " + itos(p_id));
-	_navmesh_unlink(p_id);
-	navmesh_map.erase(p_id);
+	return NavigationServer::get_singleton()->map_get_path(map, p_start, p_end, p_optimize);
 }
 
-void Navigation::_clip_path(Vector<Vector3> &path, Polygon *from_poly, const Vector3 &p_to_point, Polygon *p_to_poly) {
-
-	Vector3 from = path[path.size() - 1];
-
-	if (from.distance_to(p_to_point) < CMP_EPSILON)
-		return;
-	Plane cut_plane;
-	cut_plane.normal = (from - p_to_point).cross(up);
-	if (cut_plane.normal == Vector3())
-		return;
-	cut_plane.normal.normalize();
-	cut_plane.d = cut_plane.normal.dot(from);
-
-	while (from_poly != p_to_poly) {
-
-		int pe = from_poly->prev_edge;
-		Vector3 a = _get_vertex(from_poly->edges[pe].point);
-		Vector3 b = _get_vertex(from_poly->edges[(pe + 1) % from_poly->edges.size()].point);
-
-		from_poly = from_poly->edges[pe].C;
-		ERR_FAIL_COND(!from_poly);
-
-		if (a.distance_to(b) > CMP_EPSILON) {
+void Navigation::set_up_vector(const Vector3 &p_up) {
 
-			Vector3 inters;
-			if (cut_plane.intersects_segment(a, b, &inters)) {
-				if (inters.distance_to(p_to_point) > CMP_EPSILON && inters.distance_to(path[path.size() - 1]) > CMP_EPSILON) {
-					path.push_back(inters);
-				}
-			}
-		}
-	}
+	up = p_up;
+	NavigationServer::get_singleton()->map_set_up(map, up);
 }
 
-Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize) {
-
-	Polygon *begin_poly = NULL;
-	Polygon *end_poly = NULL;
-	Vector3 begin_point;
-	Vector3 end_point;
-	float begin_d = 1e20;
-	float end_d = 1e20;
-
-	for (Map<int, NavMesh>::Element *E = navmesh_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++) {
-
-				Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
-				Vector3 spoint = f.get_closest_point_to(p_start);
-				float dpoint = spoint.distance_to(p_start);
-				if (dpoint < begin_d) {
-					begin_d = dpoint;
-					begin_poly = &p;
-					begin_point = spoint;
-				}
-
-				spoint = f.get_closest_point_to(p_end);
-				dpoint = spoint.distance_to(p_end);
-				if (dpoint < end_d) {
-					end_d = dpoint;
-					end_poly = &p;
-					end_point = spoint;
-				}
-			}
-
-			p.prev_edge = -1;
-		}
-	}
-
-	if (!begin_poly || !end_poly) {
-
-		return Vector<Vector3>(); //no path
-	}
-
-	if (begin_poly == end_poly) {
-
-		Vector<Vector3> path;
-		path.resize(2);
-		path.write[0] = begin_point;
-		path.write[1] = end_point;
-		return path;
-	}
-
-	bool found_route = false;
-
-	List<Polygon *> open_list;
-
-	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
-			Vector3 edge[2] = {
-				_get_vertex(begin_poly->edges[i].point),
-				_get_vertex(begin_poly->edges[(i + 1) % begin_poly->edges.size()].point)
-			};
-
-			Vector3 entry = Geometry::get_closest_point_to_segment(begin_poly->entry, edge);
-			begin_poly->edges[i].C->distance = begin_point.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);
-		}
-	}
-
-	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
-			cost += p->entry.distance_to(end_point);
-#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
-
-		if (p == end_poly) {
-			//oh my reached end! stop algorithm
-			found_route = true;
-			break;
-		}
-
-		for (int i = 0; i < p->edges.size(); i++) {
-
-			Polygon::Edge &e = p->edges.write[i];
-
-			if (!e.C)
-				continue;
-
-#ifdef USE_ENTRY_POINT
-			Vector3 edge[2] = {
-				_get_vertex(p->edges[i].point),
-				_get_vertex(p->edges[(i + 1) % p->edges.size()].point)
-			};
-
-			Vector3 entry = Geometry::get_closest_point_to_segment(p->entry, edge);
-			float distance = p->entry.distance_to(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 = 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 = entry;
-#endif
-				open_list.push_back(e.C);
-			}
-		}
-
-		open_list.erase(least_cost_poly);
-	}
-
-	if (found_route) {
-
-		Vector<Vector3> path;
-
-		if (p_optimize) {
-			//string pulling
-
-			Polygon *apex_poly = end_poly;
-			Vector3 apex_point = end_point;
-			Vector3 portal_left = apex_point;
-			Vector3 portal_right = apex_point;
-			Polygon *left_poly = end_poly;
-			Polygon *right_poly = end_poly;
-			Polygon *p = end_poly;
-			path.push_back(end_point);
-
-			while (p) {
-
-				Vector3 left;
-				Vector3 right;
-
-#define CLOCK_TANGENT(m_a, m_b, m_c) (((m_a) - (m_c)).cross((m_a) - (m_b)))
-
-				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 (CLOCK_TANGENT(apex_point,left,(left+right)*0.5).dot(up) < 0){
-					if (p->clockwise) {
-						SWAP(left, right);
-					}
-				}
-
-				bool skip = false;
-
-				if (CLOCK_TANGENT(apex_point, portal_left, left).dot(up) >= 0) {
-					//process
-					if (portal_left == apex_point || CLOCK_TANGENT(apex_point, left, portal_right).dot(up) > 0) {
-						left_poly = p;
-						portal_left = left;
-					} else {
-
-						_clip_path(path, apex_poly, portal_right, right_poly);
-
-						apex_point = portal_right;
-						p = right_poly;
-						left_poly = p;
-						apex_poly = p;
-						portal_left = apex_point;
-						portal_right = apex_point;
-						path.push_back(apex_point);
-						skip = true;
-					}
-				}
-
-				if (!skip && CLOCK_TANGENT(apex_point, portal_right, right).dot(up) <= 0) {
-					//process
-					if (portal_right == apex_point || CLOCK_TANGENT(apex_point, right, portal_left).dot(up) < 0) {
-						right_poly = p;
-						portal_right = right;
-					} else {
-
-						_clip_path(path, apex_poly, portal_left, left_poly);
-
-						apex_point = portal_left;
-						p = left_poly;
-						right_poly = p;
-						apex_poly = p;
-						portal_right = apex_point;
-						portal_left = apex_point;
-						path.push_back(apex_point);
-					}
-				}
-
-				if (p != begin_poly)
-					p = p->edges[p->prev_edge].C;
-				else
-					p = NULL;
-			}
-
-			if (path[path.size() - 1] != begin_point)
-				path.push_back(begin_point);
-
-			path.invert();
-
-		} else {
-			//midpoints
-			Polygon *p = end_poly;
-
-			path.push_back(end_point);
-			while (true) {
-				int prev = p->prev_edge;
-#ifdef USE_ENTRY_POINT
-				Vector3 point = p->entry;
-#else
-				int prev_n = (p->prev_edge + 1) % p->edges.size();
-				Vector3 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point)) * 0.5;
-#endif
-				path.push_back(point);
-				p = p->edges[prev].C;
-				if (p == begin_poly)
-					break;
-			}
-
-			path.push_back(begin_point);
-
-			path.invert();
-		}
-
-		return path;
-	}
+Vector3 Navigation::get_up_vector() const {
 
-	return Vector<Vector3>();
+	return up;
 }
 
-Vector3 Navigation::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool &p_use_collision) {
-
-	bool use_collision = p_use_collision;
-	Vector3 closest_point;
-	float closest_point_d = 1e20;
-
-	for (Map<int, NavMesh>::Element *E = navmesh_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++) {
-
-				Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
-				Vector3 inters;
-				if (f.intersects_segment(p_from, p_to, &inters)) {
-
-					if (!use_collision) {
-						closest_point = inters;
-						use_collision = true;
-						closest_point_d = p_from.distance_to(inters);
-					} else if (closest_point_d > inters.distance_to(p_from)) {
-
-						closest_point = inters;
-						closest_point_d = p_from.distance_to(inters);
-					}
-				}
-			}
-
-			if (!use_collision) {
-
-				for (int i = 0; i < p.edges.size(); i++) {
-
-					Vector3 a, b;
-
-					Geometry::get_closest_points_between_segments(p_from, p_to, _get_vertex(p.edges[i].point), _get_vertex(p.edges[(i + 1) % p.edges.size()].point), a, b);
-
-					float d = a.distance_to(b);
-					if (d < closest_point_d) {
-
-						closest_point_d = d;
-						closest_point = b;
-					}
-				}
-			}
-		}
-	}
-
-	return closest_point;
+void Navigation::set_cell_size(float p_cell_size) {
+	cell_size = p_cell_size;
+	NavigationServer::get_singleton()->map_set_cell_size(map, cell_size);
 }
 
-Vector3 Navigation::get_closest_point(const Vector3 &p_point) {
-
-	Vector3 closest_point;
-	float closest_point_d = 1e20;
-
-	for (Map<int, NavMesh>::Element *E = navmesh_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++) {
-
-				Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
-				Vector3 inters = f.get_closest_point_to(p_point);
-				float d = inters.distance_to(p_point);
-				if (d < closest_point_d) {
-					closest_point = inters;
-					closest_point_d = d;
-				}
-			}
-		}
-	}
-
-	return closest_point;
+void Navigation::set_edge_connection_margin(float p_edge_connection_margin) {
+	edge_connection_margin = p_edge_connection_margin;
+	NavigationServer::get_singleton()->map_set_edge_connection_margin(map, edge_connection_margin);
 }
 
-Vector3 Navigation::get_closest_point_normal(const Vector3 &p_point) {
+void Navigation::_bind_methods() {
 
-	Vector3 closest_point;
-	Vector3 closest_normal;
-	float closest_point_d = 1e20;
+	ClassDB::bind_method(D_METHOD("get_rid"), &Navigation::get_rid);
 
-	for (Map<int, NavMesh>::Element *E = navmesh_map.front(); E; E = E->next()) {
+	ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation::get_simple_path, DEFVAL(true));
 
-		if (!E->get().linked)
-			continue;
-		for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
+	ClassDB::bind_method(D_METHOD("set_up_vector", "up"), &Navigation::set_up_vector);
+	ClassDB::bind_method(D_METHOD("get_up_vector"), &Navigation::get_up_vector);
 
-			Polygon &p = F->get();
-			for (int i = 2; i < p.edges.size(); i++) {
+	ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &Navigation::set_cell_size);
+	ClassDB::bind_method(D_METHOD("get_cell_size"), &Navigation::get_cell_size);
 
-				Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
-				Vector3 inters = f.get_closest_point_to(p_point);
-				float d = inters.distance_to(p_point);
-				if (d < closest_point_d) {
-					closest_point = inters;
-					closest_point_d = d;
-					closest_normal = f.get_plane().normal;
-				}
-			}
-		}
-	}
+	ClassDB::bind_method(D_METHOD("set_edge_connection_margin", "margin"), &Navigation::set_edge_connection_margin);
+	ClassDB::bind_method(D_METHOD("get_edge_connection_margin"), &Navigation::get_edge_connection_margin);
 
-	return closest_normal;
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "up_vector"), "set_up_vector", "get_up_vector");
+	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");
 }
 
-Object *Navigation::get_closest_point_owner(const Vector3 &p_point) {
-
-	Vector3 closest_point;
-	Object *owner = NULL;
-	float closest_point_d = 1e20;
-
-	for (Map<int, NavMesh>::Element *E = navmesh_map.front(); E; E = E->next()) {
-
-		if (!E->get().linked)
-			continue;
-		for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
+void Navigation::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_READY: {
+			NavigationServer::get_singleton()->map_set_active(map, true);
+		} break;
+		case NOTIFICATION_EXIT_TREE: {
 
-			Polygon &p = F->get();
-			for (int i = 2; i < p.edges.size(); i++) {
-
-				Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
-				Vector3 inters = f.get_closest_point_to(p_point);
-				float d = inters.distance_to(p_point);
-				if (d < closest_point_d) {
-					closest_point = inters;
-					closest_point_d = d;
-					owner = E->get().owner;
-				}
-			}
-		}
+			NavigationServer::get_singleton()->map_set_active(map, false);
+		} break;
 	}
-
-	return owner;
-}
-
-void Navigation::set_up_vector(const Vector3 &p_up) {
-
-	up = p_up;
-}
-
-Vector3 Navigation::get_up_vector() const {
-
-	return up;
 }
 
-void Navigation::_bind_methods() {
-
-	ClassDB::bind_method(D_METHOD("navmesh_add", "mesh", "xform", "owner"), &Navigation::navmesh_add, DEFVAL(Variant()));
-	ClassDB::bind_method(D_METHOD("navmesh_set_transform", "id", "xform"), &Navigation::navmesh_set_transform);
-	ClassDB::bind_method(D_METHOD("navmesh_remove", "id"), &Navigation::navmesh_remove);
+Navigation::Navigation() {
 
-	ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation::get_simple_path, DEFVAL(true));
-	ClassDB::bind_method(D_METHOD("get_closest_point_to_segment", "start", "end", "use_collision"), &Navigation::get_closest_point_to_segment, DEFVAL(false));
-	ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Navigation::get_closest_point);
-	ClassDB::bind_method(D_METHOD("get_closest_point_normal", "to_point"), &Navigation::get_closest_point_normal);
-	ClassDB::bind_method(D_METHOD("get_closest_point_owner", "to_point"), &Navigation::get_closest_point_owner);
+	map = NavigationServer::get_singleton()->map_create();
 
-	ClassDB::bind_method(D_METHOD("set_up_vector", "up"), &Navigation::set_up_vector);
-	ClassDB::bind_method(D_METHOD("get_up_vector"), &Navigation::get_up_vector);
+	set_cell_size(0.3);
+	set_edge_connection_margin(5.0); // Five meters, depends alot on the agents radius
 
-	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "up_vector"), "set_up_vector", "get_up_vector");
+	up = Vector3(0, 1, 0);
 }
 
-Navigation::Navigation() {
-
-	ERR_FAIL_COND(sizeof(Point) != 8);
-	cell_size = 0.01; //one centimeter
-	last_id = 1;
-	up = Vector3(0, 1, 0);
+Navigation::~Navigation() {
+	NavigationServer::get_singleton()->free(map);
 }
diff --git a/scene/3d/navigation.h b/scene/3d/navigation.h
index 31dbc9d4b5..68e041ad73 100644
--- a/scene/3d/navigation.h
+++ b/scene/3d/navigation.h
@@ -31,154 +31,45 @@
 #ifndef NAVIGATION_H
 #define NAVIGATION_H
 
-#include "scene/3d/navigation_mesh.h"
+#include "scene/3d/navigation_mesh_instance.h"
 #include "scene/3d/spatial.h"
 
 class Navigation : public Spatial {
 
 	GDCLASS(Navigation, Spatial);
 
-	union Point {
-
-		struct {
-			int64_t x : 21;
-			int64_t y : 22;
-			int64_t z : 21;
-		};
-
-		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;
-
-		Vector3 center;
-		Vector3 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;
-		Transform xform;
-		bool linked;
-		Ref<NavigationMesh> navmesh;
-		List<Polygon> polygons;
-	};
-
-	_FORCE_INLINE_ Point _get_point(const Vector3 &p_pos) const {
-
-		int x = int(Math::floor(p_pos.x / cell_size));
-		int y = int(Math::floor(p_pos.y / cell_size));
-		int z = int(Math::floor(p_pos.z / cell_size));
-
-		Point p;
-		p.key = 0;
-		p.x = x;
-		p.y = y;
-		p.z = z;
-		return p;
-	}
-
-	_FORCE_INLINE_ Vector3 _get_vertex(const Point &p_point) const {
-
-		return Vector3(p_point.x, p_point.y, p_point.z) * cell_size;
-	}
-
-	void _navmesh_link(int p_id);
-	void _navmesh_unlink(int p_id);
-
-	float cell_size;
-	Map<int, NavMesh> navmesh_map;
-	int last_id;
+	RID map;
 
 	Vector3 up;
-	void _clip_path(Vector<Vector3> &path, Polygon *from_poly, const Vector3 &p_to_point, Polygon *p_to_poly);
+	real_t cell_size;
+	real_t edge_connection_margin;
 
 protected:
 	static void _bind_methods();
+	void _notification(int p_what);
 
 public:
+	RID get_rid() const {
+		return map;
+	}
+
 	void set_up_vector(const Vector3 &p_up);
 	Vector3 get_up_vector() const;
 
-	//API should be as dynamic as possible
-	int navmesh_add(const Ref<NavigationMesh> &p_mesh, const Transform &p_xform, Object *p_owner = NULL);
-	void navmesh_set_transform(int p_id, const Transform &p_xform);
-	void navmesh_remove(int p_id);
+	void set_cell_size(float p_cell_size);
+	float get_cell_size() const {
+		return cell_size;
+	}
+
+	void set_edge_connection_margin(float p_edge_connection_margin);
+	float get_edge_connection_margin() const {
+		return edge_connection_margin;
+	}
 
 	Vector<Vector3> get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize = true);
-	Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool &p_use_collision = false);
-	Vector3 get_closest_point(const Vector3 &p_point);
-	Vector3 get_closest_point_normal(const Vector3 &p_point);
-	Object *get_closest_point_owner(const Vector3 &p_point);
 
 	Navigation();
+	~Navigation();
 };
 
 #endif // NAVIGATION_H
diff --git a/scene/3d/navigation_agent.cpp b/scene/3d/navigation_agent.cpp
new file mode 100644
index 0000000000..d3f447bc24
--- /dev/null
+++ b/scene/3d/navigation_agent.cpp
@@ -0,0 +1,361 @@
+/*************************************************************************/
+/*  navigation_agent.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.h"
+
+#include "core/engine.h"
+#include "scene/3d/navigation.h"
+#include "servers/navigation_server.h"
+
+void NavigationAgent::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("set_target_desired_distance", "desired_distance"), &NavigationAgent::set_target_desired_distance);
+	ClassDB::bind_method(D_METHOD("get_target_desired_distance"), &NavigationAgent::get_target_desired_distance);
+
+	ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationAgent::set_radius);
+	ClassDB::bind_method(D_METHOD("get_radius"), &NavigationAgent::get_radius);
+
+	ClassDB::bind_method(D_METHOD("set_agent_height_offset", "agent_height_offset"), &NavigationAgent::set_agent_height_offset);
+	ClassDB::bind_method(D_METHOD("get_agent_height_offset"), &NavigationAgent::get_agent_height_offset);
+
+	ClassDB::bind_method(D_METHOD("set_ignore_y", "ignore"), &NavigationAgent::set_ignore_y);
+	ClassDB::bind_method(D_METHOD("get_ignore_y"), &NavigationAgent::get_ignore_y);
+
+	ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationAgent::set_navigation_node);
+	ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationAgent::get_navigation_node);
+
+	ClassDB::bind_method(D_METHOD("set_neighbor_dist", "neighbor_dist"), &NavigationAgent::set_neighbor_dist);
+	ClassDB::bind_method(D_METHOD("get_neighbor_dist"), &NavigationAgent::get_neighbor_dist);
+
+	ClassDB::bind_method(D_METHOD("set_max_neighbors", "max_neighbors"), &NavigationAgent::set_max_neighbors);
+	ClassDB::bind_method(D_METHOD("get_max_neighbors"), &NavigationAgent::get_max_neighbors);
+
+	ClassDB::bind_method(D_METHOD("set_time_horizon", "time_horizon"), &NavigationAgent::set_time_horizon);
+	ClassDB::bind_method(D_METHOD("get_time_horizon"), &NavigationAgent::get_time_horizon);
+
+	ClassDB::bind_method(D_METHOD("set_max_speed", "max_speed"), &NavigationAgent::set_max_speed);
+	ClassDB::bind_method(D_METHOD("get_max_speed"), &NavigationAgent::get_max_speed);
+
+	ClassDB::bind_method(D_METHOD("set_path_max_distance", "max_speed"), &NavigationAgent::set_path_max_distance);
+	ClassDB::bind_method(D_METHOD("get_path_max_distance"), &NavigationAgent::get_path_max_distance);
+
+	ClassDB::bind_method(D_METHOD("set_target_location", "location"), &NavigationAgent::set_target_location);
+	ClassDB::bind_method(D_METHOD("get_target_location"), &NavigationAgent::get_target_location);
+	ClassDB::bind_method(D_METHOD("get_next_location"), &NavigationAgent::get_next_location);
+	ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent::distance_to_target);
+	ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationAgent::set_velocity);
+	ClassDB::bind_method(D_METHOD("get_nav_path"), &NavigationAgent::get_nav_path);
+	ClassDB::bind_method(D_METHOD("get_nav_path_index"), &NavigationAgent::get_nav_path_index);
+	ClassDB::bind_method(D_METHOD("is_target_reached"), &NavigationAgent::is_target_reached);
+	ClassDB::bind_method(D_METHOD("is_target_reachable"), &NavigationAgent::is_target_reachable);
+	ClassDB::bind_method(D_METHOD("is_navigation_finished"), &NavigationAgent::is_navigation_finished);
+	ClassDB::bind_method(D_METHOD("get_final_location"), &NavigationAgent::get_final_location);
+
+	ClassDB::bind_method(D_METHOD("_avoidance_done", "new_velocity"), &NavigationAgent::_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,100,0.01"), "set_radius", "get_radius");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "agent_height_offset", PROPERTY_HINT_RANGE, "-100.0,100,0.01"), "set_agent_height_offset", "get_agent_height_offset");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "neighbor_dist", PROPERTY_HINT_RANGE, "0.1,10000,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.01,100,0.01"), "set_time_horizon", "get_time_horizon");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "max_speed", PROPERTY_HINT_RANGE, "0.1,10000,0.01"), "set_max_speed", "get_max_speed");
+	ADD_PROPERTY(PropertyInfo(Variant::REAL, "path_max_distance", PROPERTY_HINT_RANGE, "0.01,100,0.1"), "set_path_max_distance", "get_path_max_distance");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "ignore_y"), "set_ignore_y", "get_ignore_y");
+
+	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 NavigationAgent::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_READY: {
+
+			agent_parent = Object::cast_to<Spatial>(get_parent());
+
+			NavigationServer::get_singleton()->agent_set_callback(agent, this, "_avoidance_done");
+
+			// Search the navigation node and set it
+			{
+				Navigation *nav = NULL;
+				Node *p = get_parent();
+				while (p != NULL) {
+					nav = Object::cast_to<Navigation>(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) {
+
+				NavigationServer::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin);
+				if (!target_reached) {
+					if (distance_to_target() < target_desired_distance) {
+						emit_signal("target_reached");
+						target_reached = true;
+					}
+				}
+			}
+		} break;
+	}
+}
+
+NavigationAgent::NavigationAgent() :
+		agent_parent(NULL),
+		navigation(NULL),
+		agent(RID()),
+		target_desired_distance(1.0),
+		navigation_height_offset(0.0),
+		path_max_distance(3.0),
+		velocity_submitted(false),
+		target_reached(false),
+		navigation_finished(true) {
+	agent = NavigationServer::get_singleton()->agent_create();
+	set_neighbor_dist(50.0);
+	set_max_neighbors(10);
+	set_time_horizon(5.0);
+	set_radius(1.0);
+	set_max_speed(10.0);
+	set_ignore_y(true);
+}
+
+NavigationAgent::~NavigationAgent() {
+	NavigationServer::get_singleton()->free(agent);
+	agent = RID(); // Pointless
+}
+
+void NavigationAgent::set_navigation(Navigation *p_nav) {
+	if (navigation == p_nav)
+		return; // Pointless
+
+	navigation = p_nav;
+	NavigationServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
+}
+
+void NavigationAgent::set_navigation_node(Node *p_nav) {
+	Navigation *nav = Object::cast_to<Navigation>(p_nav);
+	ERR_FAIL_COND(nav == NULL);
+	set_navigation(nav);
+}
+
+Node *NavigationAgent::get_navigation_node() const {
+	return Object::cast_to<Node>(navigation);
+}
+
+void NavigationAgent::set_target_desired_distance(real_t p_dd) {
+	target_desired_distance = p_dd;
+}
+
+void NavigationAgent::set_radius(real_t p_radius) {
+	radius = p_radius;
+	NavigationServer::get_singleton()->agent_set_radius(agent, radius);
+}
+
+void NavigationAgent::set_agent_height_offset(real_t p_hh) {
+	navigation_height_offset = p_hh;
+}
+
+void NavigationAgent::set_ignore_y(bool p_ignore_y) {
+	ignore_y = p_ignore_y;
+	NavigationServer::get_singleton()->agent_set_ignore_y(agent, ignore_y);
+}
+
+void NavigationAgent::set_neighbor_dist(real_t p_dist) {
+	neighbor_dist = p_dist;
+	NavigationServer::get_singleton()->agent_set_neighbor_dist(agent, neighbor_dist);
+}
+
+void NavigationAgent::set_max_neighbors(int p_count) {
+	max_neighbors = p_count;
+	NavigationServer::get_singleton()->agent_set_max_neighbors(agent, max_neighbors);
+}
+
+void NavigationAgent::set_time_horizon(real_t p_time) {
+	time_horizon = p_time;
+	NavigationServer::get_singleton()->agent_set_time_horizon(agent, time_horizon);
+}
+
+void NavigationAgent::set_max_speed(real_t p_max_speed) {
+	max_speed = p_max_speed;
+	NavigationServer::get_singleton()->agent_set_max_speed(agent, max_speed);
+}
+
+void NavigationAgent::set_path_max_distance(real_t p_pmd) {
+	path_max_distance = p_pmd;
+}
+
+real_t NavigationAgent::get_path_max_distance() {
+	return path_max_distance;
+}
+
+void NavigationAgent::set_target_location(Vector3 p_location) {
+	target_location = p_location;
+	navigation_path.clear();
+	target_reached = false;
+	navigation_finished = false;
+}
+
+Vector3 NavigationAgent::get_target_location() const {
+	return target_location;
+}
+
+Vector3 NavigationAgent::get_next_location() {
+	update_navigation();
+	if (navigation_path.size() == 0) {
+		ERR_FAIL_COND_V(agent_parent == NULL, Vector3());
+		return agent_parent->get_global_transform().origin;
+	} else {
+		return navigation_path[nav_path_index] - Vector3(0, navigation_height_offset, 0);
+	}
+}
+
+real_t NavigationAgent::distance_to_target() const {
+	ERR_FAIL_COND_V(agent_parent == NULL, 0.0);
+	return agent_parent->get_global_transform().origin.distance_to(target_location);
+}
+
+bool NavigationAgent::is_target_reached() const {
+	return target_reached;
+}
+
+bool NavigationAgent::is_target_reachable() {
+	return target_desired_distance >= get_final_location().distance_to(target_location);
+}
+
+bool NavigationAgent::is_navigation_finished() {
+	update_navigation();
+	return navigation_finished;
+}
+
+Vector3 NavigationAgent::get_final_location() {
+	update_navigation();
+	if (navigation_path.size() == 0) {
+		return Vector3();
+	}
+	return navigation_path[navigation_path.size() - 1];
+}
+
+void NavigationAgent::set_velocity(Vector3 p_velocity) {
+	target_velocity = p_velocity;
+	NavigationServer::get_singleton()->agent_set_target_velocity(agent, target_velocity);
+	NavigationServer::get_singleton()->agent_set_velocity(agent, prev_safe_velocity);
+	velocity_submitted = true;
+}
+
+void NavigationAgent::_avoidance_done(Vector3 p_new_velocity) {
+	prev_safe_velocity = p_new_velocity;
+
+	if (!velocity_submitted) {
+		target_velocity = Vector3();
+		return;
+	}
+	velocity_submitted = false;
+
+	emit_signal("velocity_computed", p_new_velocity);
+}
+
+String NavigationAgent::get_configuration_warning() const {
+	if (!Object::cast_to<Spatial>(get_parent())) {
+		return TTR("The NavigationAgent can be used only under a spatial node.");
+	}
+
+	return String();
+}
+
+void NavigationAgent::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();
+
+	Vector3 o = agent_parent->get_global_transform().origin;
+
+	bool reload_path = false;
+
+	if (NavigationServer::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) {
+			Vector3 segment[2];
+			segment[0] = navigation_path[nav_path_index - 1];
+			segment[1] = navigation_path[nav_path_index];
+			segment[0].y -= navigation_height_offset;
+			segment[1].y -= navigation_height_offset;
+			Vector3 p = Geometry::get_closest_point_to_segment(o, segment);
+			if (o.distance_to(p) >= path_max_distance) {
+				// To faraway, reload path
+				reload_path = true;
+			}
+		}
+	}
+
+	if (reload_path) {
+		navigation_path = NavigationServer::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] - Vector3(0, navigation_height_offset, 0)) < 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/3d/navigation_agent.h b/scene/3d/navigation_agent.h
new file mode 100644
index 0000000000..8f181dafe1
--- /dev/null
+++ b/scene/3d/navigation_agent.h
@@ -0,0 +1,162 @@
+/*************************************************************************/
+/*  navigation_agent.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_H
+#define NAVIGATION_AGENT_H
+
+#include "core/vector.h"
+#include "scene/main/node.h"
+
+class Spatial;
+class Navigation;
+
+class NavigationAgent : public Node {
+	GDCLASS(NavigationAgent, Node);
+
+	Spatial *agent_parent;
+	Navigation *navigation;
+
+	RID agent;
+
+	real_t target_desired_distance;
+	real_t radius;
+	real_t navigation_height_offset;
+	bool ignore_y;
+	real_t neighbor_dist;
+	int max_neighbors;
+	real_t time_horizon;
+	real_t max_speed;
+
+	real_t path_max_distance;
+
+	Vector3 target_location;
+	Vector<Vector3> navigation_path;
+	int nav_path_index;
+	bool velocity_submitted;
+	Vector3 prev_safe_velocity;
+	/// The submitted target velocity
+	Vector3 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:
+	NavigationAgent();
+	virtual ~NavigationAgent();
+
+	void set_navigation(Navigation *p_nav);
+	const Navigation *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_agent_height_offset(real_t p_hh);
+	real_t get_agent_height_offset() const {
+		return navigation_height_offset;
+	}
+
+	void set_ignore_y(bool p_ignore_y);
+	bool get_ignore_y() const {
+		return ignore_y;
+	}
+
+	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(Vector3 p_location);
+	Vector3 get_target_location() const;
+
+	Vector3 get_next_location();
+
+	Vector<Vector3> 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();
+	Vector3 get_final_location();
+
+	void set_velocity(Vector3 p_velocity);
+	void _avoidance_done(Vector3 p_new_velocity);
+
+	virtual String get_configuration_warning() const;
+
+private:
+	void update_navigation();
+};
+
+#endif
diff --git a/scene/3d/navigation_mesh_instance.cpp b/scene/3d/navigation_mesh_instance.cpp
new file mode 100644
index 0000000000..ef59767078
--- /dev/null
+++ b/scene/3d/navigation_mesh_instance.cpp
@@ -0,0 +1,258 @@
+/*************************************************************************/
+/*  navigation_mesh.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_mesh_instance.h"
+#include "core/os/thread.h"
+#include "mesh_instance.h"
+#include "navigation.h"
+#include "servers/navigation_server.h"
+
+void NavigationMeshInstance::set_enabled(bool p_enabled) {
+
+	if (enabled == p_enabled)
+		return;
+	enabled = p_enabled;
+
+	if (!is_inside_tree())
+		return;
+
+	if (!enabled) {
+
+		NavigationServer::get_singleton()->region_set_map(region, RID());
+	} else {
+
+		if (navigation) {
+
+			NavigationServer::get_singleton()->region_set_map(region, navigation->get_rid());
+		}
+	}
+
+	if (debug_view) {
+		MeshInstance *dm = Object::cast_to<MeshInstance>(debug_view);
+		if (is_enabled()) {
+			dm->set_material_override(get_tree()->get_debug_navigation_material());
+		} else {
+			dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
+		}
+	}
+
+	update_gizmo();
+}
+
+bool NavigationMeshInstance::is_enabled() const {
+
+	return enabled;
+}
+
+/////////////////////////////
+
+void NavigationMeshInstance::_notification(int p_what) {
+
+	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+
+			Spatial *c = this;
+			while (c) {
+
+				navigation = Object::cast_to<Navigation>(c);
+				if (navigation) {
+
+					if (enabled) {
+
+						NavigationServer::get_singleton()->region_set_map(region, navigation->get_rid());
+					}
+					break;
+				}
+
+				c = c->get_parent_spatial();
+			}
+
+			if (navmesh.is_valid() && get_tree()->is_debugging_navigation_hint()) {
+
+				MeshInstance *dm = memnew(MeshInstance);
+				dm->set_mesh(navmesh->get_debug_mesh());
+				if (is_enabled()) {
+					dm->set_material_override(get_tree()->get_debug_navigation_material());
+				} else {
+					dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
+				}
+				add_child(dm);
+				debug_view = dm;
+			}
+
+		} break;
+		case NOTIFICATION_TRANSFORM_CHANGED: {
+
+			NavigationServer::get_singleton()->region_set_transform(region, get_global_transform());
+
+		} break;
+		case NOTIFICATION_EXIT_TREE: {
+
+			if (navigation) {
+
+				NavigationServer::get_singleton()->region_set_map(region, RID());
+			}
+
+			if (debug_view) {
+				debug_view->queue_delete();
+				debug_view = NULL;
+			}
+			navigation = NULL;
+		} break;
+	}
+}
+
+void NavigationMeshInstance::set_navigation_mesh(const Ref<NavigationMesh> &p_navmesh) {
+
+	if (p_navmesh == navmesh)
+		return;
+
+	if (navmesh.is_valid()) {
+		navmesh->remove_change_receptor(this);
+	}
+
+	navmesh = p_navmesh;
+
+	if (navmesh.is_valid()) {
+		navmesh->add_change_receptor(this);
+	}
+
+	NavigationServer::get_singleton()->region_set_navmesh(region, p_navmesh);
+
+	if (debug_view && navmesh.is_valid()) {
+		Object::cast_to<MeshInstance>(debug_view)->set_mesh(navmesh->get_debug_mesh());
+	}
+
+	emit_signal("navigation_mesh_changed");
+
+	update_gizmo();
+	update_configuration_warning();
+}
+
+Ref<NavigationMesh> NavigationMeshInstance::get_navigation_mesh() const {
+
+	return navmesh;
+}
+
+struct BakeThreadsArgs {
+	NavigationMeshInstance *nav_mesh_instance;
+};
+
+void _bake_navigation_mesh(void *p_user_data) {
+	BakeThreadsArgs *args = static_cast<BakeThreadsArgs *>(p_user_data);
+
+	if (args->nav_mesh_instance->get_navigation_mesh().is_valid()) {
+		Ref<NavigationMesh> nav_mesh = args->nav_mesh_instance->get_navigation_mesh()->duplicate();
+
+		NavigationServer::get_singleton()->region_bake_navmesh(nav_mesh, args->nav_mesh_instance);
+		args->nav_mesh_instance->call_deferred("_bake_finished", nav_mesh);
+		memdelete(args);
+	} else {
+
+		ERR_PRINT("Can't bake the navigation mesh if the `NavigationMesh` resource doesn't exist");
+		args->nav_mesh_instance->call_deferred("_bake_finished", Ref<NavigationMesh>());
+		memdelete(args);
+	}
+}
+
+void NavigationMeshInstance::bake_navigation_mesh() {
+	ERR_FAIL_COND(bake_thread != NULL);
+
+	BakeThreadsArgs *args = memnew(BakeThreadsArgs);
+	args->nav_mesh_instance = this;
+
+	bake_thread = Thread::create(_bake_navigation_mesh, args);
+	ERR_FAIL_COND(bake_thread == NULL);
+}
+
+void NavigationMeshInstance::_bake_finished(Ref<NavigationMesh> p_nav_mesh) {
+	set_navigation_mesh(p_nav_mesh);
+	bake_thread = NULL;
+}
+
+String NavigationMeshInstance::get_configuration_warning() const {
+
+	if (!is_visible_in_tree() || !is_inside_tree())
+		return String();
+
+	if (!navmesh.is_valid()) {
+		return TTR("A NavigationMesh resource must be set or created for this node to work.");
+	}
+	const Spatial *c = this;
+	while (c) {
+
+		if (Object::cast_to<Navigation>(c))
+			return String();
+
+		c = Object::cast_to<Spatial>(c->get_parent());
+	}
+
+	return TTR("NavigationMeshInstance must be a child or grandchild to a Navigation node. It only provides navigation data.");
+}
+
+void NavigationMeshInstance::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("set_navigation_mesh", "navmesh"), &NavigationMeshInstance::set_navigation_mesh);
+	ClassDB::bind_method(D_METHOD("get_navigation_mesh"), &NavigationMeshInstance::get_navigation_mesh);
+
+	ClassDB::bind_method(D_METHOD("set_enabled", "enabled"), &NavigationMeshInstance::set_enabled);
+	ClassDB::bind_method(D_METHOD("is_enabled"), &NavigationMeshInstance::is_enabled);
+
+	ClassDB::bind_method(D_METHOD("bake_navigation_mesh"), &NavigationMeshInstance::bake_navigation_mesh);
+	ClassDB::bind_method(D_METHOD("_bake_finished", "nav_mesh"), &NavigationMeshInstance::_bake_finished);
+
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "navmesh", PROPERTY_HINT_RESOURCE_TYPE, "NavigationMesh"), "set_navigation_mesh", "get_navigation_mesh");
+	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enabled"), "set_enabled", "is_enabled");
+
+	ADD_SIGNAL(MethodInfo("navigation_mesh_changed"));
+	ADD_SIGNAL(MethodInfo("bake_finished"));
+}
+
+void NavigationMeshInstance::_changed_callback(Object *p_changed, const char *p_prop) {
+	update_gizmo();
+	update_configuration_warning();
+}
+
+NavigationMeshInstance::NavigationMeshInstance() {
+
+	enabled = true;
+	set_notify_transform(true);
+	region = NavigationServer::get_singleton()->region_create();
+
+	navigation = NULL;
+	debug_view = NULL;
+	bake_thread = NULL;
+}
+
+NavigationMeshInstance::~NavigationMeshInstance() {
+	if (navmesh.is_valid())
+		navmesh->remove_change_receptor(this);
+	NavigationServer::get_singleton()->free(region);
+}
diff --git a/scene/3d/navigation_mesh_instance.h b/scene/3d/navigation_mesh_instance.h
new file mode 100644
index 0000000000..0f23c55cc7
--- /dev/null
+++ b/scene/3d/navigation_mesh_instance.h
@@ -0,0 +1,75 @@
+/*************************************************************************/
+/*  navigation_mesh.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_MESH_INSTANCE_H
+#define NAVIGATION_MESH_INSTANCE_H
+
+#include "scene/3d/spatial.h"
+#include "scene/resources/mesh.h"
+#include "scene/resources/navigation_mesh.h"
+
+class Navigation;
+
+class NavigationMeshInstance : public Spatial {
+
+	GDCLASS(NavigationMeshInstance, Spatial);
+
+	bool enabled;
+	RID region;
+	Ref<NavigationMesh> navmesh;
+
+	Navigation *navigation;
+	Node *debug_view;
+	Thread *bake_thread;
+
+protected:
+	void _notification(int p_what);
+	static void _bind_methods();
+	void _changed_callback(Object *p_changed, const char *p_prop);
+
+public:
+	void set_enabled(bool p_enabled);
+	bool is_enabled() const;
+
+	void set_navigation_mesh(const Ref<NavigationMesh> &p_navmesh);
+	Ref<NavigationMesh> get_navigation_mesh() const;
+
+	/// Bakes the navigation mesh in a dedicated thread; once done, automatically
+	/// sets the new navigation mesh and emits a signal
+	void bake_navigation_mesh();
+	void _bake_finished(Ref<NavigationMesh> p_nav_mesh);
+
+	String get_configuration_warning() const;
+
+	NavigationMeshInstance();
+	~NavigationMeshInstance();
+};
+
+#endif // NAVIGATION_MESH_INSTANCE_H
diff --git a/scene/3d/navigation_obstacle.cpp b/scene/3d/navigation_obstacle.cpp
new file mode 100644
index 0000000000..74142bbb68
--- /dev/null
+++ b/scene/3d/navigation_obstacle.cpp
@@ -0,0 +1,163 @@
+/*************************************************************************/
+/*  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.h"
+
+#include "scene/3d/collision_shape.h"
+#include "scene/3d/navigation.h"
+#include "scene/3d/physics_body.h"
+#include "servers/navigation_server.h"
+
+void NavigationObstacle::_bind_methods() {
+
+	ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationObstacle::set_navigation_node);
+	ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationObstacle::get_navigation_node);
+}
+
+void NavigationObstacle::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_READY: {
+
+			update_agent_shape();
+
+			// Search the navigation node and set it
+			{
+				Navigation *nav = NULL;
+				Node *p = get_parent();
+				while (p != NULL) {
+					nav = Object::cast_to<Navigation>(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: {
+			Spatial *spatial = Object::cast_to<Spatial>(get_parent());
+			if (spatial) {
+				NavigationServer::get_singleton()->agent_set_position(agent, spatial->get_global_transform().origin);
+			}
+
+			PhysicsBody *rigid = Object::cast_to<PhysicsBody>(get_parent());
+			if (rigid) {
+
+				Vector3 v = rigid->get_linear_velocity();
+				NavigationServer::get_singleton()->agent_set_velocity(agent, v);
+				NavigationServer::get_singleton()->agent_set_target_velocity(agent, v);
+			}
+
+		} break;
+	}
+}
+
+NavigationObstacle::NavigationObstacle() :
+		navigation(NULL),
+		agent(RID()) {
+	agent = NavigationServer::get_singleton()->agent_create();
+}
+
+NavigationObstacle::~NavigationObstacle() {
+	NavigationServer::get_singleton()->free(agent);
+	agent = RID(); // Pointless
+}
+
+void NavigationObstacle::set_navigation(Navigation *p_nav) {
+	if (navigation == p_nav)
+		return; // Pointless
+
+	navigation = p_nav;
+	NavigationServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
+}
+
+void NavigationObstacle::set_navigation_node(Node *p_nav) {
+	Navigation *nav = Object::cast_to<Navigation>(p_nav);
+	ERR_FAIL_COND(nav == NULL);
+	set_navigation(nav);
+}
+
+Node *NavigationObstacle::get_navigation_node() const {
+	return Object::cast_to<Node>(navigation);
+}
+
+String NavigationObstacle::get_configuration_warning() const {
+	if (!Object::cast_to<Spatial>(get_parent())) {
+
+		return TTR("The NavigationObstacle only serves to provide collision avoidance to a spatial object.");
+	}
+
+	return String();
+}
+
+void NavigationObstacle::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
+		CollisionShape *cs = Object::cast_to<CollisionShape>(node->get_child(i));
+		if (cs) {
+			// Take the distance between the Body center to the shape center
+			real_t r = cs->get_transform().origin.length();
+			if (cs->get_shape().is_valid()) {
+				// and add the enclosing shape radius
+				r += cs->get_shape()->get_enclosing_radius();
+			}
+			Vector3 s = cs->get_global_transform().basis.get_scale();
+			r *= MAX(s.x, MAX(s.y, s.z));
+			// Takes the biggest radius
+			radius = MAX(radius, r);
+		}
+	}
+	Spatial *spa = Object::cast_to<Spatial>(node);
+	if (spa) {
+		Vector3 s = spa->get_global_transform().basis.get_scale();
+		radius *= MAX(s.x, MAX(s.y, s.z));
+	}
+
+	if (radius == 0.0)
+		radius = 1.0; // Never a 0 radius
+
+	// Initialize the Agent as an object
+	NavigationServer::get_singleton()->agent_set_neighbor_dist(agent, 0.0);
+	NavigationServer::get_singleton()->agent_set_max_neighbors(agent, 0);
+	NavigationServer::get_singleton()->agent_set_time_horizon(agent, 0.0);
+	NavigationServer::get_singleton()->agent_set_radius(agent, radius);
+	NavigationServer::get_singleton()->agent_set_max_speed(agent, 0.0);
+}
diff --git a/scene/3d/navigation_obstacle.h b/scene/3d/navigation_obstacle.h
new file mode 100644
index 0000000000..34f8153614
--- /dev/null
+++ b/scene/3d/navigation_obstacle.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_H
+#define NAVIGATION_OBSTACLE_H
+
+#include "scene/main/node.h"
+
+class Navigation;
+
+class NavigationObstacle : public Node {
+	GDCLASS(NavigationObstacle, Node);
+
+	Navigation *navigation;
+
+	RID agent;
+
+protected:
+	static void _bind_methods();
+	void _notification(int p_what);
+
+public:
+	NavigationObstacle();
+	virtual ~NavigationObstacle();
+
+	void set_navigation(Navigation *p_nav);
+	const Navigation *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/3d/physics_body.cpp b/scene/3d/physics_body.cpp
index a1a221b5bb..fbfd372272 100644
--- a/scene/3d/physics_body.cpp
+++ b/scene/3d/physics_body.cpp
@@ -36,15 +36,14 @@
 #include "core/method_bind_ext.gen.inc"
 #include "core/object.h"
 #include "core/rid.h"
+#include "scene/3d/collision_shape.h"
 #include "scene/scene_string_names.h"
+#include "servers/navigation_server.h"
 
 #ifdef TOOLS_ENABLED
 #include "editor/plugins/spatial_editor_plugin.h"
 #endif
 
-void PhysicsBody::_notification(int p_what) {
-}
-
 Vector3 PhysicsBody::get_linear_velocity() const {
 
 	return Vector3();
@@ -1104,6 +1103,14 @@ Ref<KinematicCollision> KinematicBody::_move(const Vector3 &p_motion, bool p_inf
 	return Ref<KinematicCollision>();
 }
 
+Vector3 KinematicBody::get_linear_velocity() const {
+	return linear_velocity;
+}
+
+Vector3 KinematicBody::get_angular_velocity() const {
+	return angular_velocity;
+}
+
 bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes, bool p_test_only) {
 
 	Transform gt = get_global_transform();
@@ -1399,6 +1406,8 @@ void KinematicBody::_notification(int p_what) {
 
 void KinematicBody::_bind_methods() {
 
+	ClassDB::bind_method(D_METHOD("_direct_state_changed"), &KinematicBody::_direct_state_changed);
+
 	ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "test_only"), &KinematicBody::_move, DEFVAL(true), DEFVAL(true), DEFVAL(false));
 	ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "up_direction", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true));
 	ClassDB::bind_method(D_METHOD("move_and_slide_with_snap", "linear_velocity", "snap", "up_direction", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide_with_snap, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true));
@@ -1427,6 +1436,17 @@ void KinematicBody::_bind_methods() {
 	ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision/safe_margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_safe_margin", "get_safe_margin");
 }
 
+void KinematicBody::_direct_state_changed(Object *p_state) {
+#ifdef DEBUG_ENABLED
+	PhysicsDirectBodyState *state = Object::cast_to<PhysicsDirectBodyState>(p_state);
+#else
+	PhysicsDirectBodyState *state = (PhysicsDirectBodyState *)p_state; //trust it
+#endif
+
+	linear_velocity = state->get_linear_velocity();
+	angular_velocity = state->get_angular_velocity();
+}
+
 KinematicBody::KinematicBody() :
 		PhysicsBody(PhysicsServer::BODY_MODE_KINEMATIC) {
 
@@ -1435,6 +1455,8 @@ KinematicBody::KinematicBody() :
 	on_floor = false;
 	on_ceiling = false;
 	on_wall = false;
+
+	PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(), this, "_direct_state_changed");
 }
 KinematicBody::~KinematicBody() {
 
diff --git a/scene/3d/physics_body.h b/scene/3d/physics_body.h
index 6a1e803eaf..0ee877d887 100644
--- a/scene/3d/physics_body.h
+++ b/scene/3d/physics_body.h
@@ -49,7 +49,6 @@ class PhysicsBody : public CollisionObject {
 
 protected:
 	static void _bind_methods();
-	void _notification(int p_what);
 	PhysicsBody(PhysicsServer::BodyMode p_mode);
 
 public:
@@ -296,6 +295,9 @@ public:
 	};
 
 private:
+	Vector3 linear_velocity;
+	Vector3 angular_velocity;
+
 	uint16_t locked_axis;
 
 	float margin;
@@ -319,7 +321,12 @@ protected:
 	void _notification(int p_what);
 	static void _bind_methods();
 
+	virtual void _direct_state_changed(Object *p_state);
+
 public:
+	virtual Vector3 get_linear_velocity() const;
+	virtual Vector3 get_angular_velocity() const;
+
 	bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
 	bool test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia);
 
diff --git a/scene/main/scene_tree.cpp b/scene/main/scene_tree.cpp
index c0e566eed5..15d21859ea 100644
--- a/scene/main/scene_tree.cpp
+++ b/scene/main/scene_tree.cpp
@@ -46,6 +46,7 @@
 #include "scene/resources/mesh.h"
 #include "scene/resources/packed_scene.h"
 #include "scene/scene_string_names.h"
+#include "servers/navigation_server.h"
 #include "servers/physics_2d_server.h"
 #include "servers/physics_server.h"
 #include "viewport.h"
@@ -896,6 +897,7 @@ void SceneTree::set_pause(bool p_enabled) {
 	if (p_enabled == pause)
 		return;
 	pause = p_enabled;
+	NavigationServer::get_singleton()->set_active(!p_enabled);
 	PhysicsServer::get_singleton()->set_active(!p_enabled);
 	Physics2DServer::get_singleton()->set_active(!p_enabled);
 	if (get_root())
diff --git a/scene/register_scene_types.cpp b/scene/register_scene_types.cpp
index d53872df6e..1cd4b54a5b 100644
--- a/scene/register_scene_types.cpp
+++ b/scene/register_scene_types.cpp
@@ -50,6 +50,8 @@
 #include "scene/2d/mesh_instance_2d.h"
 #include "scene/2d/multimesh_instance_2d.h"
 #include "scene/2d/navigation_2d.h"
+#include "scene/2d/navigation_agent_2d.h"
+#include "scene/2d/navigation_obstacle_2d.h"
 #include "scene/2d/parallax_background.h"
 #include "scene/2d/parallax_layer.h"
 #include "scene/2d/particles_2d.h"
@@ -145,6 +147,7 @@
 #include "scene/resources/material.h"
 #include "scene/resources/mesh.h"
 #include "scene/resources/mesh_data_tool.h"
+#include "scene/resources/navigation_mesh.h"
 #include "scene/resources/packed_scene.h"
 #include "scene/resources/particles_material.h"
 #include "scene/resources/physics_material.h"
@@ -168,9 +171,6 @@
 #include "scene/resources/world_2d.h"
 #include "scene/scene_string_names.h"
 
-#include "scene/3d/spatial.h"
-#include "scene/3d/world_environment.h"
-
 #ifndef _3D_DISABLED
 #include "scene/3d/area.h"
 #include "scene/3d/arvr_nodes.h"
@@ -189,7 +189,9 @@
 #include "scene/3d/mesh_instance.h"
 #include "scene/3d/multimesh_instance.h"
 #include "scene/3d/navigation.h"
-#include "scene/3d/navigation_mesh.h"
+#include "scene/3d/navigation_agent.h"
+#include "scene/3d/navigation_mesh_instance.h"
+#include "scene/3d/navigation_obstacle.h"
 #include "scene/3d/particles.h"
 #include "scene/3d/path.h"
 #include "scene/3d/physics_body.h"
@@ -201,10 +203,12 @@
 #include "scene/3d/remote_transform.h"
 #include "scene/3d/skeleton.h"
 #include "scene/3d/soft_body.h"
+#include "scene/3d/spatial.h"
 #include "scene/3d/spring_arm.h"
 #include "scene/3d/sprite_3d.h"
 #include "scene/3d/vehicle_body.h"
 #include "scene/3d/visibility_notifier.h"
+#include "scene/3d/world_environment.h"
 #include "scene/animation/skeleton_ik.h"
 #include "scene/resources/environment.h"
 #include "scene/resources/mesh_library.h"
@@ -422,9 +426,6 @@ void register_scene_types() {
 	ClassDB::register_class<Particles>();
 	ClassDB::register_class<CPUParticles>();
 	ClassDB::register_class<Position3D>();
-	ClassDB::register_class<NavigationMeshInstance>();
-	ClassDB::register_class<NavigationMesh>();
-	ClassDB::register_class<Navigation>();
 
 	ClassDB::register_class<RootMotionView>();
 	ClassDB::set_class_enabled("RootMotionView", false); //disabled by default, enabled by editor
@@ -469,9 +470,15 @@ void register_scene_types() {
 	ClassDB::register_class<ConeTwistJoint>();
 	ClassDB::register_class<Generic6DOFJoint>();
 
+	ClassDB::register_class<Navigation>();
+	ClassDB::register_class<NavigationMeshInstance>();
+	ClassDB::register_class<NavigationAgent>();
+	ClassDB::register_class<NavigationObstacle>();
+
 	OS::get_singleton()->yield(); //may take time to init
 
 #endif
+	ClassDB::register_class<NavigationMesh>();
 
 	AcceptDialog::set_swap_ok_cancel(GLOBAL_DEF("gui/common/swap_ok_cancel", bool(OS::get_singleton()->get_swap_ok_cancel())));
 
@@ -713,6 +720,8 @@ void register_scene_types() {
 	ClassDB::register_class<Navigation2D>();
 	ClassDB::register_class<NavigationPolygon>();
 	ClassDB::register_class<NavigationPolygonInstance>();
+	ClassDB::register_class<NavigationAgent2D>();
+	ClassDB::register_class<NavigationObstacle2D>();
 
 	OS::get_singleton()->yield(); //may take time to init
 
diff --git a/scene/resources/box_shape.cpp b/scene/resources/box_shape.cpp
index dee3943b8e..e1f485bae6 100644
--- a/scene/resources/box_shape.cpp
+++ b/scene/resources/box_shape.cpp
@@ -48,6 +48,10 @@ Vector<Vector3> BoxShape::get_debug_mesh_lines() {
 	return lines;
 }
 
+real_t BoxShape::get_enclosing_radius() const {
+	return extents.length();
+}
+
 void BoxShape::_update_shape() {
 
 	PhysicsServer::get_singleton()->shape_set_data(get_shape(), extents);
diff --git a/scene/resources/box_shape.h b/scene/resources/box_shape.h
index b577a70e1e..fb164cc300 100644
--- a/scene/resources/box_shape.h
+++ b/scene/resources/box_shape.h
@@ -48,6 +48,7 @@ public:
 	Vector3 get_extents() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	BoxShape();
 };
diff --git a/scene/resources/capsule_shape.cpp b/scene/resources/capsule_shape.cpp
index 61331a336d..933129936a 100644
--- a/scene/resources/capsule_shape.cpp
+++ b/scene/resources/capsule_shape.cpp
@@ -69,6 +69,10 @@ Vector<Vector3> CapsuleShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t CapsuleShape::get_enclosing_radius() const {
+	return radius + height * 0.5;
+}
+
 void CapsuleShape::_update_shape() {
 
 	Dictionary d;
diff --git a/scene/resources/capsule_shape.h b/scene/resources/capsule_shape.h
index 8e7d7bcb94..f097e51175 100644
--- a/scene/resources/capsule_shape.h
+++ b/scene/resources/capsule_shape.h
@@ -51,6 +51,7 @@ public:
 	float get_height() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	CapsuleShape();
 };
diff --git a/scene/resources/capsule_shape_2d.cpp b/scene/resources/capsule_shape_2d.cpp
index 008e8bb13d..5658395cee 100644
--- a/scene/resources/capsule_shape_2d.cpp
+++ b/scene/resources/capsule_shape_2d.cpp
@@ -97,6 +97,10 @@ Rect2 CapsuleShape2D::get_rect() const {
 	return rect;
 }
 
+real_t CapsuleShape2D::get_enclosing_radius() const {
+	return radius + height * 0.5;
+}
+
 void CapsuleShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_radius", "radius"), &CapsuleShape2D::set_radius);
diff --git a/scene/resources/capsule_shape_2d.h b/scene/resources/capsule_shape_2d.h
index 8398fab839..fe401a610c 100644
--- a/scene/resources/capsule_shape_2d.h
+++ b/scene/resources/capsule_shape_2d.h
@@ -56,6 +56,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	CapsuleShape2D();
 };
diff --git a/scene/resources/circle_shape_2d.cpp b/scene/resources/circle_shape_2d.cpp
index 7142309e28..10f8ab8a8a 100644
--- a/scene/resources/circle_shape_2d.cpp
+++ b/scene/resources/circle_shape_2d.cpp
@@ -70,6 +70,10 @@ Rect2 CircleShape2D::get_rect() const {
 	return rect;
 }
 
+real_t CircleShape2D::get_enclosing_radius() const {
+	return radius;
+}
+
 void CircleShape2D::draw(const RID &p_to_rid, const Color &p_color) {
 
 	Vector<Vector2> points;
diff --git a/scene/resources/circle_shape_2d.h b/scene/resources/circle_shape_2d.h
index cafd291f13..8b064f4d9f 100644
--- a/scene/resources/circle_shape_2d.h
+++ b/scene/resources/circle_shape_2d.h
@@ -50,6 +50,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	CircleShape2D();
 };
diff --git a/scene/resources/concave_polygon_shape.cpp b/scene/resources/concave_polygon_shape.cpp
index 48cc08eae4..0a93f99ea3 100644
--- a/scene/resources/concave_polygon_shape.cpp
+++ b/scene/resources/concave_polygon_shape.cpp
@@ -64,6 +64,16 @@ Vector<Vector3> ConcavePolygonShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t ConcavePolygonShape::get_enclosing_radius() const {
+	PoolVector<Vector3> data = get_faces();
+	PoolVector<Vector3>::Read read = data.read();
+	real_t r = 0;
+	for (int i(0); i < data.size(); i++) {
+		r = MAX(read[i].length_squared(), r);
+	}
+	return Math::sqrt(r);
+}
+
 void ConcavePolygonShape::_update_shape() {
 	Shape::_update_shape();
 }
diff --git a/scene/resources/concave_polygon_shape.h b/scene/resources/concave_polygon_shape.h
index 5e371954d4..b4bebbd7b4 100644
--- a/scene/resources/concave_polygon_shape.h
+++ b/scene/resources/concave_polygon_shape.h
@@ -66,7 +66,8 @@ public:
 	void set_faces(const PoolVector<Vector3> &p_faces);
 	PoolVector<Vector3> get_faces() const;
 
-	Vector<Vector3> get_debug_mesh_lines();
+	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	ConcavePolygonShape();
 };
diff --git a/scene/resources/concave_polygon_shape_2d.cpp b/scene/resources/concave_polygon_shape_2d.cpp
index 640f395158..840733add3 100644
--- a/scene/resources/concave_polygon_shape_2d.cpp
+++ b/scene/resources/concave_polygon_shape_2d.cpp
@@ -94,6 +94,16 @@ Rect2 ConcavePolygonShape2D::get_rect() const {
 	return rect;
 }
 
+real_t ConcavePolygonShape2D::get_enclosing_radius() const {
+	PoolVector<Vector2> data = get_segments();
+	PoolVector<Vector2>::Read read = data.read();
+	real_t r = 0;
+	for (int i(0); i < data.size(); i++) {
+		r = MAX(read[i].length_squared(), r);
+	}
+	return Math::sqrt(r);
+}
+
 void ConcavePolygonShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_segments", "segments"), &ConcavePolygonShape2D::set_segments);
diff --git a/scene/resources/concave_polygon_shape_2d.h b/scene/resources/concave_polygon_shape_2d.h
index b95ed100e7..4e47ad34b8 100644
--- a/scene/resources/concave_polygon_shape_2d.h
+++ b/scene/resources/concave_polygon_shape_2d.h
@@ -47,6 +47,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	ConcavePolygonShape2D();
 };
diff --git a/scene/resources/convex_polygon_shape.cpp b/scene/resources/convex_polygon_shape.cpp
index 1498eb4af9..21fdcc1f06 100644
--- a/scene/resources/convex_polygon_shape.cpp
+++ b/scene/resources/convex_polygon_shape.cpp
@@ -55,6 +55,16 @@ Vector<Vector3> ConvexPolygonShape::get_debug_mesh_lines() {
 	return Vector<Vector3>();
 }
 
+real_t ConvexPolygonShape::get_enclosing_radius() const {
+	PoolVector<Vector3> data = get_points();
+	PoolVector<Vector3>::Read read = data.read();
+	real_t r = 0;
+	for (int i(0); i < data.size(); i++) {
+		r = MAX(read[i].length_squared(), r);
+	}
+	return Math::sqrt(r);
+}
+
 void ConvexPolygonShape::_update_shape() {
 
 	PhysicsServer::get_singleton()->shape_set_data(get_shape(), points);
diff --git a/scene/resources/convex_polygon_shape.h b/scene/resources/convex_polygon_shape.h
index 4725d09b26..e3bf02399a 100644
--- a/scene/resources/convex_polygon_shape.h
+++ b/scene/resources/convex_polygon_shape.h
@@ -48,6 +48,7 @@ public:
 	PoolVector<Vector3> get_points() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	ConvexPolygonShape();
 };
diff --git a/scene/resources/convex_polygon_shape_2d.cpp b/scene/resources/convex_polygon_shape_2d.cpp
index c404190398..296d014cc7 100644
--- a/scene/resources/convex_polygon_shape_2d.cpp
+++ b/scene/resources/convex_polygon_shape_2d.cpp
@@ -97,6 +97,14 @@ Rect2 ConvexPolygonShape2D::get_rect() const {
 	return rect;
 }
 
+real_t ConvexPolygonShape2D::get_enclosing_radius() const {
+	real_t r = 0;
+	for (int i(0); i < get_points().size(); i++) {
+		r = MAX(get_points()[i].length_squared(), r);
+	}
+	return Math::sqrt(r);
+}
+
 ConvexPolygonShape2D::ConvexPolygonShape2D() :
 		Shape2D(Physics2DServer::get_singleton()->convex_polygon_shape_create()) {
 }
diff --git a/scene/resources/convex_polygon_shape_2d.h b/scene/resources/convex_polygon_shape_2d.h
index ed6be738ab..83c250c0ce 100644
--- a/scene/resources/convex_polygon_shape_2d.h
+++ b/scene/resources/convex_polygon_shape_2d.h
@@ -51,6 +51,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	ConvexPolygonShape2D();
 };
diff --git a/scene/resources/cylinder_shape.cpp b/scene/resources/cylinder_shape.cpp
index d9f6e4f054..b9b0d77a1b 100644
--- a/scene/resources/cylinder_shape.cpp
+++ b/scene/resources/cylinder_shape.cpp
@@ -62,6 +62,10 @@ Vector<Vector3> CylinderShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t CylinderShape::get_enclosing_radius() const {
+	return Vector2(radius, height * 0.5).length();
+}
+
 void CylinderShape::_update_shape() {
 
 	Dictionary d;
diff --git a/scene/resources/cylinder_shape.h b/scene/resources/cylinder_shape.h
index ebddd4d92f..a26fda10cd 100644
--- a/scene/resources/cylinder_shape.h
+++ b/scene/resources/cylinder_shape.h
@@ -50,6 +50,7 @@ public:
 	float get_height() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	CylinderShape();
 };
diff --git a/scene/resources/height_map_shape.cpp b/scene/resources/height_map_shape.cpp
index 2b86d658d8..48c9221e27 100644
--- a/scene/resources/height_map_shape.cpp
+++ b/scene/resources/height_map_shape.cpp
@@ -76,6 +76,10 @@ Vector<Vector3> HeightMapShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t HeightMapShape::get_enclosing_radius() const {
+	return Vector3(real_t(map_width), max_height - min_height, real_t(map_depth)).length();
+}
+
 void HeightMapShape::_update_shape() {
 
 	Dictionary d;
diff --git a/scene/resources/height_map_shape.h b/scene/resources/height_map_shape.h
index 3a976e3add..a6263f061f 100644
--- a/scene/resources/height_map_shape.h
+++ b/scene/resources/height_map_shape.h
@@ -55,6 +55,7 @@ public:
 	PoolRealArray get_map_data() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	HeightMapShape();
 };
diff --git a/scene/resources/line_shape_2d.cpp b/scene/resources/line_shape_2d.cpp
index 84da8125ea..d1bb61820b 100644
--- a/scene/resources/line_shape_2d.cpp
+++ b/scene/resources/line_shape_2d.cpp
@@ -100,6 +100,10 @@ Rect2 LineShape2D::get_rect() const {
 	return rect;
 }
 
+real_t LineShape2D::get_enclosing_radius() const {
+	return d;
+}
+
 void LineShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_normal", "normal"), &LineShape2D::set_normal);
diff --git a/scene/resources/line_shape_2d.h b/scene/resources/line_shape_2d.h
index 7babfe12f6..5bf9e85bb9 100644
--- a/scene/resources/line_shape_2d.h
+++ b/scene/resources/line_shape_2d.h
@@ -55,6 +55,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	LineShape2D();
 };
diff --git a/scene/resources/mesh_library.h b/scene/resources/mesh_library.h
index 471db74175..5f1fddae22 100644
--- a/scene/resources/mesh_library.h
+++ b/scene/resources/mesh_library.h
@@ -34,7 +34,7 @@
 #include "core/map.h"
 #include "core/resource.h"
 #include "mesh.h"
-#include "scene/3d/navigation_mesh.h"
+#include "scene/3d/navigation_mesh_instance.h"
 #include "shape.h"
 
 class MeshLibrary : public Resource {
diff --git a/scene/3d/navigation_mesh.cpp b/scene/resources/navigation_mesh.cpp
index aaba91125e..e6544778bc 100644
--- a/scene/3d/navigation_mesh.cpp
+++ b/scene/resources/navigation_mesh.cpp
@@ -29,8 +29,6 @@
 /*************************************************************************/
 
 #include "navigation_mesh.h"
-#include "mesh_instance.h"
-#include "navigation.h"
 
 void NavigationMesh::create_from_mesh(const Ref<Mesh> &p_mesh) {
 
@@ -548,197 +546,3 @@ NavigationMesh::NavigationMesh() {
 	filter_ledge_spans = false;
 	filter_walkable_low_height_spans = false;
 }
-
-void NavigationMeshInstance::set_enabled(bool p_enabled) {
-
-	if (enabled == p_enabled)
-		return;
-	enabled = p_enabled;
-
-	if (!is_inside_tree())
-		return;
-
-	if (!enabled) {
-
-		if (nav_id != -1) {
-			navigation->navmesh_remove(nav_id);
-			nav_id = -1;
-		}
-	} else {
-
-		if (navigation) {
-
-			if (navmesh.is_valid()) {
-
-				nav_id = navigation->navmesh_add(navmesh, get_relative_transform(navigation), this);
-			}
-		}
-	}
-
-	if (debug_view) {
-		MeshInstance *dm = Object::cast_to<MeshInstance>(debug_view);
-		if (is_enabled()) {
-			dm->set_material_override(get_tree()->get_debug_navigation_material());
-		} else {
-			dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
-		}
-	}
-
-	update_gizmo();
-}
-
-bool NavigationMeshInstance::is_enabled() const {
-
-	return enabled;
-}
-
-/////////////////////////////
-
-void NavigationMeshInstance::_notification(int p_what) {
-
-	switch (p_what) {
-		case NOTIFICATION_ENTER_TREE: {
-
-			Spatial *c = this;
-			while (c) {
-
-				navigation = Object::cast_to<Navigation>(c);
-				if (navigation) {
-
-					if (enabled && navmesh.is_valid()) {
-
-						nav_id = navigation->navmesh_add(navmesh, get_relative_transform(navigation), this);
-					}
-					break;
-				}
-
-				c = c->get_parent_spatial();
-			}
-
-			if (navmesh.is_valid() && get_tree()->is_debugging_navigation_hint()) {
-
-				MeshInstance *dm = memnew(MeshInstance);
-				dm->set_mesh(navmesh->get_debug_mesh());
-				if (is_enabled()) {
-					dm->set_material_override(get_tree()->get_debug_navigation_material());
-				} else {
-					dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
-				}
-				add_child(dm);
-				debug_view = dm;
-			}
-
-		} break;
-		case NOTIFICATION_TRANSFORM_CHANGED: {
-
-			if (navigation && nav_id != -1) {
-				navigation->navmesh_set_transform(nav_id, get_relative_transform(navigation));
-			}
-
-		} break;
-		case NOTIFICATION_EXIT_TREE: {
-
-			if (navigation) {
-
-				if (nav_id != -1) {
-					navigation->navmesh_remove(nav_id);
-					nav_id = -1;
-				}
-			}
-
-			if (debug_view) {
-				debug_view->queue_delete();
-				debug_view = NULL;
-			}
-			navigation = NULL;
-		} break;
-	}
-}
-
-void NavigationMeshInstance::set_navigation_mesh(const Ref<NavigationMesh> &p_navmesh) {
-
-	if (p_navmesh == navmesh)
-		return;
-
-	if (navigation && nav_id != -1) {
-		navigation->navmesh_remove(nav_id);
-		nav_id = -1;
-	}
-
-	if (navmesh.is_valid()) {
-		navmesh->remove_change_receptor(this);
-	}
-
-	navmesh = p_navmesh;
-
-	if (navmesh.is_valid()) {
-		navmesh->add_change_receptor(this);
-	}
-
-	if (navigation && navmesh.is_valid() && enabled) {
-		nav_id = navigation->navmesh_add(navmesh, get_relative_transform(navigation), this);
-	}
-
-	if (debug_view && navmesh.is_valid()) {
-		Object::cast_to<MeshInstance>(debug_view)->set_mesh(navmesh->get_debug_mesh());
-	}
-
-	update_gizmo();
-	update_configuration_warning();
-}
-
-Ref<NavigationMesh> NavigationMeshInstance::get_navigation_mesh() const {
-
-	return navmesh;
-}
-
-String NavigationMeshInstance::get_configuration_warning() const {
-
-	if (!is_visible_in_tree() || !is_inside_tree())
-		return String();
-
-	if (!navmesh.is_valid()) {
-		return TTR("A NavigationMesh resource must be set or created for this node to work.");
-	}
-	const Spatial *c = this;
-	while (c) {
-
-		if (Object::cast_to<Navigation>(c))
-			return String();
-
-		c = Object::cast_to<Spatial>(c->get_parent());
-	}
-
-	return TTR("NavigationMeshInstance must be a child or grandchild to a Navigation node. It only provides navigation data.");
-}
-
-void NavigationMeshInstance::_bind_methods() {
-
-	ClassDB::bind_method(D_METHOD("set_navigation_mesh", "navmesh"), &NavigationMeshInstance::set_navigation_mesh);
-	ClassDB::bind_method(D_METHOD("get_navigation_mesh"), &NavigationMeshInstance::get_navigation_mesh);
-
-	ClassDB::bind_method(D_METHOD("set_enabled", "enabled"), &NavigationMeshInstance::set_enabled);
-	ClassDB::bind_method(D_METHOD("is_enabled"), &NavigationMeshInstance::is_enabled);
-
-	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "navmesh", PROPERTY_HINT_RESOURCE_TYPE, "NavigationMesh"), "set_navigation_mesh", "get_navigation_mesh");
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enabled"), "set_enabled", "is_enabled");
-}
-
-void NavigationMeshInstance::_changed_callback(Object *p_changed, const char *p_prop) {
-	update_gizmo();
-	update_configuration_warning();
-}
-
-NavigationMeshInstance::NavigationMeshInstance() {
-
-	debug_view = NULL;
-	navigation = NULL;
-	nav_id = -1;
-	enabled = true;
-	set_notify_transform(true);
-}
-
-NavigationMeshInstance::~NavigationMeshInstance() {
-	if (navmesh.is_valid())
-		navmesh->remove_change_receptor(this);
-}
diff --git a/scene/3d/navigation_mesh.h b/scene/resources/navigation_mesh.h
index f9ab911bea..a2b1c62eab 100644
--- a/scene/3d/navigation_mesh.h
+++ b/scene/resources/navigation_mesh.h
@@ -31,7 +31,6 @@
 #ifndef NAVIGATION_MESH_H
 #define NAVIGATION_MESH_H
 
-#include "scene/3d/spatial.h"
 #include "scene/resources/mesh.h"
 
 class Mesh;
@@ -193,35 +192,4 @@ public:
 	NavigationMesh();
 };
 
-class Navigation;
-
-class NavigationMeshInstance : public Spatial {
-
-	GDCLASS(NavigationMeshInstance, Spatial);
-
-	bool enabled;
-	int nav_id;
-	Navigation *navigation;
-	Ref<NavigationMesh> navmesh;
-
-	Node *debug_view;
-
-protected:
-	void _notification(int p_what);
-	static void _bind_methods();
-	void _changed_callback(Object *p_changed, const char *p_prop);
-
-public:
-	void set_enabled(bool p_enabled);
-	bool is_enabled() const;
-
-	void set_navigation_mesh(const Ref<NavigationMesh> &p_navmesh);
-	Ref<NavigationMesh> get_navigation_mesh() const;
-
-	String get_configuration_warning() const;
-
-	NavigationMeshInstance();
-	~NavigationMeshInstance();
-};
-
 #endif // NAVIGATION_MESH_H
diff --git a/scene/resources/plane_shape.h b/scene/resources/plane_shape.h
index 8bea1268e5..360f9dbbe9 100644
--- a/scene/resources/plane_shape.h
+++ b/scene/resources/plane_shape.h
@@ -47,6 +47,10 @@ public:
 	Plane get_plane() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const {
+		// Should be infinite?
+		return 0;
+	}
 
 	PlaneShape();
 };
diff --git a/scene/resources/ray_shape.cpp b/scene/resources/ray_shape.cpp
index 3062e96293..1a9b7e6dd2 100644
--- a/scene/resources/ray_shape.cpp
+++ b/scene/resources/ray_shape.cpp
@@ -41,6 +41,10 @@ Vector<Vector3> RayShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t RayShape::get_enclosing_radius() const {
+	return length;
+}
+
 void RayShape::_update_shape() {
 
 	Dictionary d;
diff --git a/scene/resources/ray_shape.h b/scene/resources/ray_shape.h
index ddf9f56ea3..c89705ad7d 100644
--- a/scene/resources/ray_shape.h
+++ b/scene/resources/ray_shape.h
@@ -50,6 +50,7 @@ public:
 	bool get_slips_on_slope() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	RayShape();
 };
diff --git a/scene/resources/rectangle_shape_2d.cpp b/scene/resources/rectangle_shape_2d.cpp
index 0030e6dd4f..f8c8ffb289 100644
--- a/scene/resources/rectangle_shape_2d.cpp
+++ b/scene/resources/rectangle_shape_2d.cpp
@@ -59,6 +59,10 @@ Rect2 RectangleShape2D::get_rect() const {
 	return Rect2(-extents, extents * 2.0);
 }
 
+real_t RectangleShape2D::get_enclosing_radius() const {
+	return extents.length();
+}
+
 void RectangleShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_extents", "extents"), &RectangleShape2D::set_extents);
diff --git a/scene/resources/rectangle_shape_2d.h b/scene/resources/rectangle_shape_2d.h
index 686b421a2a..68fc539085 100644
--- a/scene/resources/rectangle_shape_2d.h
+++ b/scene/resources/rectangle_shape_2d.h
@@ -48,6 +48,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	RectangleShape2D();
 };
diff --git a/scene/resources/segment_shape_2d.cpp b/scene/resources/segment_shape_2d.cpp
index 0070de72a2..3094d0f9bd 100644
--- a/scene/resources/segment_shape_2d.cpp
+++ b/scene/resources/segment_shape_2d.cpp
@@ -82,6 +82,10 @@ Rect2 SegmentShape2D::get_rect() const {
 	return rect;
 }
 
+real_t SegmentShape2D::get_enclosing_radius() const {
+	return (a + b).length();
+}
+
 void SegmentShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_a", "a"), &SegmentShape2D::set_a);
@@ -138,6 +142,10 @@ Rect2 RayShape2D::get_rect() const {
 	return rect;
 }
 
+real_t RayShape2D::get_enclosing_radius() const {
+	return length;
+}
+
 void RayShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_length", "length"), &RayShape2D::set_length);
diff --git a/scene/resources/segment_shape_2d.h b/scene/resources/segment_shape_2d.h
index aaf838ebfb..ca10c24f07 100644
--- a/scene/resources/segment_shape_2d.h
+++ b/scene/resources/segment_shape_2d.h
@@ -55,6 +55,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	SegmentShape2D();
 };
@@ -79,6 +80,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	RayShape2D();
 };
diff --git a/scene/resources/shape.h b/scene/resources/shape.h
index 8fc265f3bc..227a581c96 100644
--- a/scene/resources/shape.h
+++ b/scene/resources/shape.h
@@ -32,6 +32,7 @@
 #define SHAPE_H
 
 #include "core/resource.h"
+
 class ArrayMesh;
 
 class Shape : public Resource {
@@ -57,6 +58,8 @@ public:
 
 	Ref<ArrayMesh> get_debug_mesh();
 	virtual Vector<Vector3> get_debug_mesh_lines() = 0; // { return Vector<Vector3>(); }
+	/// Returns the radius of a sphere that fully enclose this shape
+	virtual real_t get_enclosing_radius() const = 0;
 
 	void add_vertices_to_array(PoolVector<Vector3> &array, const Transform &p_xform);
 
diff --git a/scene/resources/shape_2d.h b/scene/resources/shape_2d.h
index 13ad7492ae..e2933ec031 100644
--- a/scene/resources/shape_2d.h
+++ b/scene/resources/shape_2d.h
@@ -58,6 +58,8 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color) {}
 	virtual Rect2 get_rect() const { return Rect2(); }
+	/// Returns the radius of a circle that fully enclose this shape
+	virtual real_t get_enclosing_radius() const = 0;
 	virtual RID get_rid() const;
 	Shape2D();
 	~Shape2D();
diff --git a/scene/resources/sphere_shape.cpp b/scene/resources/sphere_shape.cpp
index f408717834..56121b6941 100644
--- a/scene/resources/sphere_shape.cpp
+++ b/scene/resources/sphere_shape.cpp
@@ -55,6 +55,10 @@ Vector<Vector3> SphereShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t SphereShape::get_enclosing_radius() const {
+	return radius;
+}
+
 void SphereShape::_update_shape() {
 
 	PhysicsServer::get_singleton()->shape_set_data(get_shape(), radius);
diff --git a/scene/resources/sphere_shape.h b/scene/resources/sphere_shape.h
index 6002a3baeb..07e8f1e233 100644
--- a/scene/resources/sphere_shape.h
+++ b/scene/resources/sphere_shape.h
@@ -48,6 +48,7 @@ public:
 	float get_radius() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	SphereShape();
 };
diff --git a/scene/resources/world.cpp b/scene/resources/world.cpp
index 1099852098..e1e3974016 100644
--- a/scene/resources/world.cpp
+++ b/scene/resources/world.cpp
@@ -262,6 +262,7 @@ RID World::get_space() const {
 
 	return space;
 }
+
 RID World::get_scenario() const {
 
 	return scenario;