- Integrated NavigationServer and Navigation2DServer.

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

This work has been kindly sponsored by IMVU.

12 files changed, 1183 insertions, 1766 deletions

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..80d7aeba8f
--- /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_player_center_height", "player_center_height"), &NavigationAgent::set_player_center_height);
+	ClassDB::bind_method(D_METHOD("get_player_center_height"), &NavigationAgent::get_player_center_height);
+
+	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, "player_center_height", PROPERTY_HINT_RANGE, "-100.0,100,0.01"), "set_player_center_height", "get_player_center_height");
+	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_player_center_height(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..a0b86d6931
--- /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_player_center_height(real_t p_hh);
+	real_t get_player_center_height() 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.cpp b/scene/3d/navigation_mesh.cpp
deleted file mode 100644
index aaba91125e..0000000000
--- a/scene/3d/navigation_mesh.cpp
+++ /dev/null
@@ -1,744 +0,0 @@
-/*************************************************************************/
-/*  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.h"
-#include "mesh_instance.h"
-#include "navigation.h"
-
-void NavigationMesh::create_from_mesh(const Ref<Mesh> &p_mesh) {
-
-	vertices = PoolVector<Vector3>();
-	clear_polygons();
-
-	for (int i = 0; i < p_mesh->get_surface_count(); i++) {
-
-		if (p_mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES)
-			continue;
-		Array arr = p_mesh->surface_get_arrays(i);
-		PoolVector<Vector3> varr = arr[Mesh::ARRAY_VERTEX];
-		PoolVector<int> iarr = arr[Mesh::ARRAY_INDEX];
-		if (varr.size() == 0 || iarr.size() == 0)
-			continue;
-
-		int from = vertices.size();
-		vertices.append_array(varr);
-		int rlen = iarr.size();
-		PoolVector<int>::Read r = iarr.read();
-
-		for (int j = 0; j < rlen; j += 3) {
-			Vector<int> vi;
-			vi.resize(3);
-			vi.write[0] = r[j + 0] + from;
-			vi.write[1] = r[j + 1] + from;
-			vi.write[2] = r[j + 2] + from;
-
-			add_polygon(vi);
-		}
-	}
-}
-
-void NavigationMesh::set_sample_partition_type(int p_value) {
-	ERR_FAIL_COND(p_value >= SAMPLE_PARTITION_MAX);
-	partition_type = static_cast<SamplePartitionType>(p_value);
-}
-
-int NavigationMesh::get_sample_partition_type() const {
-	return static_cast<int>(partition_type);
-}
-
-void NavigationMesh::set_parsed_geometry_type(int p_value) {
-	ERR_FAIL_COND(p_value >= PARSED_GEOMETRY_MAX);
-	parsed_geometry_type = static_cast<ParsedGeometryType>(p_value);
-	_change_notify();
-}
-
-int NavigationMesh::get_parsed_geometry_type() const {
-	return parsed_geometry_type;
-}
-
-void NavigationMesh::set_collision_mask(uint32_t p_mask) {
-
-	collision_mask = p_mask;
-}
-
-uint32_t NavigationMesh::get_collision_mask() const {
-
-	return collision_mask;
-}
-
-void NavigationMesh::set_collision_mask_bit(int p_bit, bool p_value) {
-
-	uint32_t mask = get_collision_mask();
-	if (p_value)
-		mask |= 1 << p_bit;
-	else
-		mask &= ~(1 << p_bit);
-	set_collision_mask(mask);
-}
-
-bool NavigationMesh::get_collision_mask_bit(int p_bit) const {
-
-	return get_collision_mask() & (1 << p_bit);
-}
-
-void NavigationMesh::set_source_geometry_mode(int p_geometry_mode) {
-	ERR_FAIL_INDEX(p_geometry_mode, SOURCE_GEOMETRY_MAX);
-	source_geometry_mode = static_cast<SourceGeometryMode>(p_geometry_mode);
-	_change_notify();
-}
-
-int NavigationMesh::get_source_geometry_mode() const {
-	return source_geometry_mode;
-}
-
-void NavigationMesh::set_source_group_name(StringName p_group_name) {
-	source_group_name = p_group_name;
-}
-
-StringName NavigationMesh::get_source_group_name() const {
-	return source_group_name;
-}
-
-void NavigationMesh::set_cell_size(float p_value) {
-	cell_size = p_value;
-}
-
-float NavigationMesh::get_cell_size() const {
-	return cell_size;
-}
-
-void NavigationMesh::set_cell_height(float p_value) {
-	cell_height = p_value;
-}
-
-float NavigationMesh::get_cell_height() const {
-	return cell_height;
-}
-
-void NavigationMesh::set_agent_height(float p_value) {
-	agent_height = p_value;
-}
-
-float NavigationMesh::get_agent_height() const {
-	return agent_height;
-}
-
-void NavigationMesh::set_agent_radius(float p_value) {
-	agent_radius = p_value;
-}
-
-float NavigationMesh::get_agent_radius() {
-	return agent_radius;
-}
-
-void NavigationMesh::set_agent_max_climb(float p_value) {
-	agent_max_climb = p_value;
-}
-
-float NavigationMesh::get_agent_max_climb() const {
-	return agent_max_climb;
-}
-
-void NavigationMesh::set_agent_max_slope(float p_value) {
-	agent_max_slope = p_value;
-}
-
-float NavigationMesh::get_agent_max_slope() const {
-	return agent_max_slope;
-}
-
-void NavigationMesh::set_region_min_size(float p_value) {
-	region_min_size = p_value;
-}
-
-float NavigationMesh::get_region_min_size() const {
-	return region_min_size;
-}
-
-void NavigationMesh::set_region_merge_size(float p_value) {
-	region_merge_size = p_value;
-}
-
-float NavigationMesh::get_region_merge_size() const {
-	return region_merge_size;
-}
-
-void NavigationMesh::set_edge_max_length(float p_value) {
-	edge_max_length = p_value;
-}
-
-float NavigationMesh::get_edge_max_length() const {
-	return edge_max_length;
-}
-
-void NavigationMesh::set_edge_max_error(float p_value) {
-	edge_max_error = p_value;
-}
-
-float NavigationMesh::get_edge_max_error() const {
-	return edge_max_error;
-}
-
-void NavigationMesh::set_verts_per_poly(float p_value) {
-	verts_per_poly = p_value;
-}
-
-float NavigationMesh::get_verts_per_poly() const {
-	return verts_per_poly;
-}
-
-void NavigationMesh::set_detail_sample_distance(float p_value) {
-	detail_sample_distance = p_value;
-}
-
-float NavigationMesh::get_detail_sample_distance() const {
-	return detail_sample_distance;
-}
-
-void NavigationMesh::set_detail_sample_max_error(float p_value) {
-	detail_sample_max_error = p_value;
-}
-
-float NavigationMesh::get_detail_sample_max_error() const {
-	return detail_sample_max_error;
-}
-
-void NavigationMesh::set_filter_low_hanging_obstacles(bool p_value) {
-	filter_low_hanging_obstacles = p_value;
-}
-
-bool NavigationMesh::get_filter_low_hanging_obstacles() const {
-	return filter_low_hanging_obstacles;
-}
-
-void NavigationMesh::set_filter_ledge_spans(bool p_value) {
-	filter_ledge_spans = p_value;
-}
-
-bool NavigationMesh::get_filter_ledge_spans() const {
-	return filter_ledge_spans;
-}
-
-void NavigationMesh::set_filter_walkable_low_height_spans(bool p_value) {
-	filter_walkable_low_height_spans = p_value;
-}
-
-bool NavigationMesh::get_filter_walkable_low_height_spans() const {
-	return filter_walkable_low_height_spans;
-}
-
-void NavigationMesh::set_vertices(const PoolVector<Vector3> &p_vertices) {
-
-	vertices = p_vertices;
-	_change_notify();
-}
-
-PoolVector<Vector3> NavigationMesh::get_vertices() const {
-
-	return vertices;
-}
-
-void NavigationMesh::_set_polygons(const Array &p_array) {
-
-	polygons.resize(p_array.size());
-	for (int i = 0; i < p_array.size(); i++) {
-		polygons.write[i].indices = p_array[i];
-	}
-	_change_notify();
-}
-
-Array NavigationMesh::_get_polygons() const {
-
-	Array ret;
-	ret.resize(polygons.size());
-	for (int i = 0; i < ret.size(); i++) {
-		ret[i] = polygons[i].indices;
-	}
-
-	return ret;
-}
-
-void NavigationMesh::add_polygon(const Vector<int> &p_polygon) {
-
-	Polygon polygon;
-	polygon.indices = p_polygon;
-	polygons.push_back(polygon);
-	_change_notify();
-}
-int NavigationMesh::get_polygon_count() const {
-
-	return polygons.size();
-}
-Vector<int> NavigationMesh::get_polygon(int p_idx) {
-
-	ERR_FAIL_INDEX_V(p_idx, polygons.size(), Vector<int>());
-	return polygons[p_idx].indices;
-}
-void NavigationMesh::clear_polygons() {
-
-	polygons.clear();
-}
-
-Ref<Mesh> NavigationMesh::get_debug_mesh() {
-
-	if (debug_mesh.is_valid())
-		return debug_mesh;
-
-	PoolVector<Vector3> vertices = get_vertices();
-	PoolVector<Vector3>::Read vr = vertices.read();
-	List<Face3> faces;
-	for (int i = 0; i < get_polygon_count(); i++) {
-		Vector<int> p = get_polygon(i);
-
-		for (int j = 2; j < p.size(); j++) {
-			Face3 f;
-			f.vertex[0] = vr[p[0]];
-			f.vertex[1] = vr[p[j - 1]];
-			f.vertex[2] = vr[p[j]];
-
-			faces.push_back(f);
-		}
-	}
-
-	Map<_EdgeKey, bool> edge_map;
-	PoolVector<Vector3> tmeshfaces;
-	tmeshfaces.resize(faces.size() * 3);
-
-	{
-		PoolVector<Vector3>::Write tw = tmeshfaces.write();
-		int tidx = 0;
-
-		for (List<Face3>::Element *E = faces.front(); E; E = E->next()) {
-
-			const Face3 &f = E->get();
-
-			for (int j = 0; j < 3; j++) {
-
-				tw[tidx++] = f.vertex[j];
-				_EdgeKey ek;
-				ek.from = f.vertex[j].snapped(Vector3(CMP_EPSILON, CMP_EPSILON, CMP_EPSILON));
-				ek.to = f.vertex[(j + 1) % 3].snapped(Vector3(CMP_EPSILON, CMP_EPSILON, CMP_EPSILON));
-				if (ek.from < ek.to)
-					SWAP(ek.from, ek.to);
-
-				Map<_EdgeKey, bool>::Element *F = edge_map.find(ek);
-
-				if (F) {
-
-					F->get() = false;
-
-				} else {
-
-					edge_map[ek] = true;
-				}
-			}
-		}
-	}
-	List<Vector3> lines;
-
-	for (Map<_EdgeKey, bool>::Element *E = edge_map.front(); E; E = E->next()) {
-
-		if (E->get()) {
-			lines.push_back(E->key().from);
-			lines.push_back(E->key().to);
-		}
-	}
-
-	PoolVector<Vector3> varr;
-	varr.resize(lines.size());
-	{
-		PoolVector<Vector3>::Write w = varr.write();
-		int idx = 0;
-		for (List<Vector3>::Element *E = lines.front(); E; E = E->next()) {
-			w[idx++] = E->get();
-		}
-	}
-
-	debug_mesh = Ref<ArrayMesh>(memnew(ArrayMesh));
-
-	Array arr;
-	arr.resize(Mesh::ARRAY_MAX);
-	arr[Mesh::ARRAY_VERTEX] = varr;
-
-	debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, arr);
-
-	return debug_mesh;
-}
-
-void NavigationMesh::_bind_methods() {
-	ClassDB::bind_method(D_METHOD("set_sample_partition_type", "sample_partition_type"), &NavigationMesh::set_sample_partition_type);
-	ClassDB::bind_method(D_METHOD("get_sample_partition_type"), &NavigationMesh::get_sample_partition_type);
-
-	ClassDB::bind_method(D_METHOD("set_parsed_geometry_type", "geometry_type"), &NavigationMesh::set_parsed_geometry_type);
-	ClassDB::bind_method(D_METHOD("get_parsed_geometry_type"), &NavigationMesh::get_parsed_geometry_type);
-
-	ClassDB::bind_method(D_METHOD("set_collision_mask", "mask"), &NavigationMesh::set_collision_mask);
-	ClassDB::bind_method(D_METHOD("get_collision_mask"), &NavigationMesh::get_collision_mask);
-
-	ClassDB::bind_method(D_METHOD("set_collision_mask_bit", "bit", "value"), &NavigationMesh::set_collision_mask_bit);
-	ClassDB::bind_method(D_METHOD("get_collision_mask_bit", "bit"), &NavigationMesh::get_collision_mask_bit);
-
-	ClassDB::bind_method(D_METHOD("set_source_geometry_mode", "mask"), &NavigationMesh::set_source_geometry_mode);
-	ClassDB::bind_method(D_METHOD("get_source_geometry_mode"), &NavigationMesh::get_source_geometry_mode);
-
-	ClassDB::bind_method(D_METHOD("set_source_group_name", "mask"), &NavigationMesh::set_source_group_name);
-	ClassDB::bind_method(D_METHOD("get_source_group_name"), &NavigationMesh::get_source_group_name);
-
-	ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &NavigationMesh::set_cell_size);
-	ClassDB::bind_method(D_METHOD("get_cell_size"), &NavigationMesh::get_cell_size);
-
-	ClassDB::bind_method(D_METHOD("set_cell_height", "cell_height"), &NavigationMesh::set_cell_height);
-	ClassDB::bind_method(D_METHOD("get_cell_height"), &NavigationMesh::get_cell_height);
-
-	ClassDB::bind_method(D_METHOD("set_agent_height", "agent_height"), &NavigationMesh::set_agent_height);
-	ClassDB::bind_method(D_METHOD("get_agent_height"), &NavigationMesh::get_agent_height);
-
-	ClassDB::bind_method(D_METHOD("set_agent_radius", "agent_radius"), &NavigationMesh::set_agent_radius);
-	ClassDB::bind_method(D_METHOD("get_agent_radius"), &NavigationMesh::get_agent_radius);
-
-	ClassDB::bind_method(D_METHOD("set_agent_max_climb", "agent_max_climb"), &NavigationMesh::set_agent_max_climb);
-	ClassDB::bind_method(D_METHOD("get_agent_max_climb"), &NavigationMesh::get_agent_max_climb);
-
-	ClassDB::bind_method(D_METHOD("set_agent_max_slope", "agent_max_slope"), &NavigationMesh::set_agent_max_slope);
-	ClassDB::bind_method(D_METHOD("get_agent_max_slope"), &NavigationMesh::get_agent_max_slope);
-
-	ClassDB::bind_method(D_METHOD("set_region_min_size", "region_min_size"), &NavigationMesh::set_region_min_size);
-	ClassDB::bind_method(D_METHOD("get_region_min_size"), &NavigationMesh::get_region_min_size);
-
-	ClassDB::bind_method(D_METHOD("set_region_merge_size", "region_merge_size"), &NavigationMesh::set_region_merge_size);
-	ClassDB::bind_method(D_METHOD("get_region_merge_size"), &NavigationMesh::get_region_merge_size);
-
-	ClassDB::bind_method(D_METHOD("set_edge_max_length", "edge_max_length"), &NavigationMesh::set_edge_max_length);
-	ClassDB::bind_method(D_METHOD("get_edge_max_length"), &NavigationMesh::get_edge_max_length);
-
-	ClassDB::bind_method(D_METHOD("set_edge_max_error", "edge_max_error"), &NavigationMesh::set_edge_max_error);
-	ClassDB::bind_method(D_METHOD("get_edge_max_error"), &NavigationMesh::get_edge_max_error);
-
-	ClassDB::bind_method(D_METHOD("set_verts_per_poly", "verts_per_poly"), &NavigationMesh::set_verts_per_poly);
-	ClassDB::bind_method(D_METHOD("get_verts_per_poly"), &NavigationMesh::get_verts_per_poly);
-
-	ClassDB::bind_method(D_METHOD("set_detail_sample_distance", "detail_sample_dist"), &NavigationMesh::set_detail_sample_distance);
-	ClassDB::bind_method(D_METHOD("get_detail_sample_distance"), &NavigationMesh::get_detail_sample_distance);
-
-	ClassDB::bind_method(D_METHOD("set_detail_sample_max_error", "detail_sample_max_error"), &NavigationMesh::set_detail_sample_max_error);
-	ClassDB::bind_method(D_METHOD("get_detail_sample_max_error"), &NavigationMesh::get_detail_sample_max_error);
-
-	ClassDB::bind_method(D_METHOD("set_filter_low_hanging_obstacles", "filter_low_hanging_obstacles"), &NavigationMesh::set_filter_low_hanging_obstacles);
-	ClassDB::bind_method(D_METHOD("get_filter_low_hanging_obstacles"), &NavigationMesh::get_filter_low_hanging_obstacles);
-
-	ClassDB::bind_method(D_METHOD("set_filter_ledge_spans", "filter_ledge_spans"), &NavigationMesh::set_filter_ledge_spans);
-	ClassDB::bind_method(D_METHOD("get_filter_ledge_spans"), &NavigationMesh::get_filter_ledge_spans);
-
-	ClassDB::bind_method(D_METHOD("set_filter_walkable_low_height_spans", "filter_walkable_low_height_spans"), &NavigationMesh::set_filter_walkable_low_height_spans);
-	ClassDB::bind_method(D_METHOD("get_filter_walkable_low_height_spans"), &NavigationMesh::get_filter_walkable_low_height_spans);
-
-	ClassDB::bind_method(D_METHOD("set_vertices", "vertices"), &NavigationMesh::set_vertices);
-	ClassDB::bind_method(D_METHOD("get_vertices"), &NavigationMesh::get_vertices);
-
-	ClassDB::bind_method(D_METHOD("add_polygon", "polygon"), &NavigationMesh::add_polygon);
-	ClassDB::bind_method(D_METHOD("get_polygon_count"), &NavigationMesh::get_polygon_count);
-	ClassDB::bind_method(D_METHOD("get_polygon", "idx"), &NavigationMesh::get_polygon);
-	ClassDB::bind_method(D_METHOD("clear_polygons"), &NavigationMesh::clear_polygons);
-
-	ClassDB::bind_method(D_METHOD("create_from_mesh", "mesh"), &NavigationMesh::create_from_mesh);
-
-	ClassDB::bind_method(D_METHOD("_set_polygons", "polygons"), &NavigationMesh::_set_polygons);
-	ClassDB::bind_method(D_METHOD("_get_polygons"), &NavigationMesh::_get_polygons);
-
-	BIND_CONSTANT(SAMPLE_PARTITION_WATERSHED);
-	BIND_CONSTANT(SAMPLE_PARTITION_MONOTONE);
-	BIND_CONSTANT(SAMPLE_PARTITION_LAYERS);
-
-	BIND_CONSTANT(PARSED_GEOMETRY_MESH_INSTANCES);
-	BIND_CONSTANT(PARSED_GEOMETRY_STATIC_COLLIDERS);
-	BIND_CONSTANT(PARSED_GEOMETRY_BOTH);
-
-	ADD_PROPERTY(PropertyInfo(Variant::POOL_VECTOR3_ARRAY, "vertices", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR | PROPERTY_USAGE_INTERNAL), "set_vertices", "get_vertices");
-	ADD_PROPERTY(PropertyInfo(Variant::ARRAY, "polygons", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR | PROPERTY_USAGE_INTERNAL), "_set_polygons", "_get_polygons");
-
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "sample_partition_type/sample_partition_type", PROPERTY_HINT_ENUM, "Watershed,Monotone,Layers"), "set_sample_partition_type", "get_sample_partition_type");
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "geometry/parsed_geometry_type", PROPERTY_HINT_ENUM, "Mesh Instances,Static Colliders,Both"), "set_parsed_geometry_type", "get_parsed_geometry_type");
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "geometry/collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask");
-	ADD_PROPERTY(PropertyInfo(Variant::INT, "geometry/source_geometry_mode", PROPERTY_HINT_ENUM, "Navmesh Children, Group With Children, Group Explicit"), "set_source_geometry_mode", "get_source_geometry_mode");
-	ADD_PROPERTY(PropertyInfo(Variant::STRING, "geometry/source_group_name"), "set_source_group_name", "get_source_group_name");
-
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "cell/size", PROPERTY_HINT_RANGE, "0.1,1.0,0.01,or_greater"), "set_cell_size", "get_cell_size");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "cell/height", PROPERTY_HINT_RANGE, "0.1,1.0,0.01,or_greater"), "set_cell_height", "get_cell_height");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "agent/height", PROPERTY_HINT_RANGE, "0.1,5.0,0.01,or_greater"), "set_agent_height", "get_agent_height");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "agent/radius", PROPERTY_HINT_RANGE, "0.1,5.0,0.01,or_greater"), "set_agent_radius", "get_agent_radius");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "agent/max_climb", PROPERTY_HINT_RANGE, "0.1,5.0,0.01,or_greater"), "set_agent_max_climb", "get_agent_max_climb");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "agent/max_slope", PROPERTY_HINT_RANGE, "0.0,90.0,0.1"), "set_agent_max_slope", "get_agent_max_slope");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "region/min_size", PROPERTY_HINT_RANGE, "0.0,150.0,0.01,or_greater"), "set_region_min_size", "get_region_min_size");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "region/merge_size", PROPERTY_HINT_RANGE, "0.0,150.0,0.01,or_greater"), "set_region_merge_size", "get_region_merge_size");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "edge/max_length", PROPERTY_HINT_RANGE, "0.0,50.0,0.01,or_greater"), "set_edge_max_length", "get_edge_max_length");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "edge/max_error", PROPERTY_HINT_RANGE, "0.1,3.0,0.01,or_greater"), "set_edge_max_error", "get_edge_max_error");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "polygon/verts_per_poly", PROPERTY_HINT_RANGE, "3.0,12.0,1.0,or_greater"), "set_verts_per_poly", "get_verts_per_poly");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "detail/sample_distance", PROPERTY_HINT_RANGE, "0.0,16.0,0.01,or_greater"), "set_detail_sample_distance", "get_detail_sample_distance");
-	ADD_PROPERTY(PropertyInfo(Variant::REAL, "detail/sample_max_error", PROPERTY_HINT_RANGE, "0.0,16.0,0.01,or_greater"), "set_detail_sample_max_error", "get_detail_sample_max_error");
-
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "filter/low_hanging_obstacles"), "set_filter_low_hanging_obstacles", "get_filter_low_hanging_obstacles");
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "filter/ledge_spans"), "set_filter_ledge_spans", "get_filter_ledge_spans");
-	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "filter/filter_walkable_low_height_spans"), "set_filter_walkable_low_height_spans", "get_filter_walkable_low_height_spans");
-}
-
-void NavigationMesh::_validate_property(PropertyInfo &property) const {
-	if (property.name == "geometry/collision_mask") {
-		if (parsed_geometry_type == PARSED_GEOMETRY_MESH_INSTANCES) {
-			property.usage = 0;
-			return;
-		}
-	}
-
-	if (property.name == "geometry/source_group_name") {
-		if (source_geometry_mode == SOURCE_GEOMETRY_NAVMESH_CHILDREN) {
-			property.usage = 0;
-			return;
-		}
-	}
-}
-
-NavigationMesh::NavigationMesh() {
-	cell_size = 0.3f;
-	cell_height = 0.2f;
-	agent_height = 2.0f;
-	agent_radius = 0.6f;
-	agent_max_climb = 0.9f;
-	agent_max_slope = 45.0f;
-	region_min_size = 8.0f;
-	region_merge_size = 20.0f;
-	edge_max_length = 12.0f;
-	edge_max_error = 1.3f;
-	verts_per_poly = 6.0f;
-	detail_sample_distance = 6.0f;
-	detail_sample_max_error = 1.0f;
-
-	partition_type = SAMPLE_PARTITION_WATERSHED;
-	parsed_geometry_type = PARSED_GEOMETRY_MESH_INSTANCES;
-	collision_mask = 0xFFFFFFFF;
-	source_geometry_mode = SOURCE_GEOMETRY_NAVMESH_CHILDREN;
-	source_group_name = "navmesh";
-	filter_low_hanging_obstacles = false;
-	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/3d/navigation_mesh.h
deleted file mode 100644
index f9ab911bea..0000000000
--- a/scene/3d/navigation_mesh.h
+++ /dev/null
@@ -1,227 +0,0 @@
-/*************************************************************************/
-/*  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_H
-#define NAVIGATION_MESH_H
-
-#include "scene/3d/spatial.h"
-#include "scene/resources/mesh.h"
-
-class Mesh;
-
-class NavigationMesh : public Resource {
-
-	GDCLASS(NavigationMesh, Resource);
-
-	PoolVector<Vector3> vertices;
-	struct Polygon {
-		Vector<int> indices;
-	};
-	Vector<Polygon> polygons;
-	Ref<ArrayMesh> debug_mesh;
-
-	struct _EdgeKey {
-
-		Vector3 from;
-		Vector3 to;
-
-		bool operator<(const _EdgeKey &p_with) const { return from == p_with.from ? to < p_with.to : from < p_with.from; }
-	};
-
-protected:
-	static void _bind_methods();
-	virtual void _validate_property(PropertyInfo &property) const;
-
-	void _set_polygons(const Array &p_array);
-	Array _get_polygons() const;
-
-public:
-	enum SamplePartitionType {
-		SAMPLE_PARTITION_WATERSHED = 0,
-		SAMPLE_PARTITION_MONOTONE,
-		SAMPLE_PARTITION_LAYERS,
-		SAMPLE_PARTITION_MAX
-	};
-
-	enum ParsedGeometryType {
-		PARSED_GEOMETRY_MESH_INSTANCES = 0,
-		PARSED_GEOMETRY_STATIC_COLLIDERS,
-		PARSED_GEOMETRY_BOTH,
-		PARSED_GEOMETRY_MAX
-	};
-
-	enum SourceGeometryMode {
-		SOURCE_GEOMETRY_NAVMESH_CHILDREN = 0,
-		SOURCE_GEOMETRY_GROUPS_WITH_CHILDREN,
-		SOURCE_GEOMETRY_GROUPS_EXPLICIT,
-		SOURCE_GEOMETRY_MAX
-	};
-
-protected:
-	float cell_size;
-	float cell_height;
-	float agent_height;
-	float agent_radius;
-	float agent_max_climb;
-	float agent_max_slope;
-	float region_min_size;
-	float region_merge_size;
-	float edge_max_length;
-	float edge_max_error;
-	float verts_per_poly;
-	float detail_sample_distance;
-	float detail_sample_max_error;
-
-	SamplePartitionType partition_type;
-	ParsedGeometryType parsed_geometry_type;
-	uint32_t collision_mask;
-
-	SourceGeometryMode source_geometry_mode;
-	StringName source_group_name;
-
-	bool filter_low_hanging_obstacles;
-	bool filter_ledge_spans;
-	bool filter_walkable_low_height_spans;
-
-public:
-	// Recast settings
-	void set_sample_partition_type(int p_value);
-	int get_sample_partition_type() const;
-
-	void set_parsed_geometry_type(int p_value);
-	int get_parsed_geometry_type() const;
-
-	void set_collision_mask(uint32_t p_mask);
-	uint32_t get_collision_mask() const;
-
-	void set_collision_mask_bit(int p_bit, bool p_value);
-	bool get_collision_mask_bit(int p_bit) const;
-
-	void set_source_geometry_mode(int p_geometry_mode);
-	int get_source_geometry_mode() const;
-
-	void set_source_group_name(StringName p_group_name);
-	StringName get_source_group_name() const;
-
-	void set_cell_size(float p_value);
-	float get_cell_size() const;
-
-	void set_cell_height(float p_value);
-	float get_cell_height() const;
-
-	void set_agent_height(float p_value);
-	float get_agent_height() const;
-
-	void set_agent_radius(float p_value);
-	float get_agent_radius();
-
-	void set_agent_max_climb(float p_value);
-	float get_agent_max_climb() const;
-
-	void set_agent_max_slope(float p_value);
-	float get_agent_max_slope() const;
-
-	void set_region_min_size(float p_value);
-	float get_region_min_size() const;
-
-	void set_region_merge_size(float p_value);
-	float get_region_merge_size() const;
-
-	void set_edge_max_length(float p_value);
-	float get_edge_max_length() const;
-
-	void set_edge_max_error(float p_value);
-	float get_edge_max_error() const;
-
-	void set_verts_per_poly(float p_value);
-	float get_verts_per_poly() const;
-
-	void set_detail_sample_distance(float p_value);
-	float get_detail_sample_distance() const;
-
-	void set_detail_sample_max_error(float p_value);
-	float get_detail_sample_max_error() const;
-
-	void set_filter_low_hanging_obstacles(bool p_value);
-	bool get_filter_low_hanging_obstacles() const;
-
-	void set_filter_ledge_spans(bool p_value);
-	bool get_filter_ledge_spans() const;
-
-	void set_filter_walkable_low_height_spans(bool p_value);
-	bool get_filter_walkable_low_height_spans() const;
-
-	void create_from_mesh(const Ref<Mesh> &p_mesh);
-
-	void set_vertices(const PoolVector<Vector3> &p_vertices);
-	PoolVector<Vector3> get_vertices() const;
-
-	void add_polygon(const Vector<int> &p_polygon);
-	int get_polygon_count() const;
-	Vector<int> get_polygon(int p_idx);
-	void clear_polygons();
-
-	Ref<Mesh> get_debug_mesh();
-
-	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/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);