Merge pull request #34776 from AndreaCatania/nav_pr

Integrated the new `NavigationServer` and `NavigationServer2D`

37 files changed, 857 insertions, 2 deletions

diff --git a/scene/resources/box_shape.cpp b/scene/resources/box_shape.cpp
index dee3943b8e..e1f485bae6 100644
--- a/scene/resources/box_shape.cpp
+++ b/scene/resources/box_shape.cpp
@@ -48,6 +48,10 @@ Vector<Vector3> BoxShape::get_debug_mesh_lines() {
 	return lines;
 }
 
+real_t BoxShape::get_enclosing_radius() const {
+	return extents.length();
+}
+
 void BoxShape::_update_shape() {
 
 	PhysicsServer::get_singleton()->shape_set_data(get_shape(), extents);
diff --git a/scene/resources/box_shape.h b/scene/resources/box_shape.h
index b577a70e1e..fb164cc300 100644
--- a/scene/resources/box_shape.h
+++ b/scene/resources/box_shape.h
@@ -48,6 +48,7 @@ public:
 	Vector3 get_extents() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	BoxShape();
 };
diff --git a/scene/resources/capsule_shape.cpp b/scene/resources/capsule_shape.cpp
index 61331a336d..933129936a 100644
--- a/scene/resources/capsule_shape.cpp
+++ b/scene/resources/capsule_shape.cpp
@@ -69,6 +69,10 @@ Vector<Vector3> CapsuleShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t CapsuleShape::get_enclosing_radius() const {
+	return radius + height * 0.5;
+}
+
 void CapsuleShape::_update_shape() {
 
 	Dictionary d;
diff --git a/scene/resources/capsule_shape.h b/scene/resources/capsule_shape.h
index 8e7d7bcb94..f097e51175 100644
--- a/scene/resources/capsule_shape.h
+++ b/scene/resources/capsule_shape.h
@@ -51,6 +51,7 @@ public:
 	float get_height() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	CapsuleShape();
 };
diff --git a/scene/resources/capsule_shape_2d.cpp b/scene/resources/capsule_shape_2d.cpp
index 008e8bb13d..5658395cee 100644
--- a/scene/resources/capsule_shape_2d.cpp
+++ b/scene/resources/capsule_shape_2d.cpp
@@ -97,6 +97,10 @@ Rect2 CapsuleShape2D::get_rect() const {
 	return rect;
 }
 
+real_t CapsuleShape2D::get_enclosing_radius() const {
+	return radius + height * 0.5;
+}
+
 void CapsuleShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_radius", "radius"), &CapsuleShape2D::set_radius);
diff --git a/scene/resources/capsule_shape_2d.h b/scene/resources/capsule_shape_2d.h
index 8398fab839..fe401a610c 100644
--- a/scene/resources/capsule_shape_2d.h
+++ b/scene/resources/capsule_shape_2d.h
@@ -56,6 +56,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	CapsuleShape2D();
 };
diff --git a/scene/resources/circle_shape_2d.cpp b/scene/resources/circle_shape_2d.cpp
index 7142309e28..10f8ab8a8a 100644
--- a/scene/resources/circle_shape_2d.cpp
+++ b/scene/resources/circle_shape_2d.cpp
@@ -70,6 +70,10 @@ Rect2 CircleShape2D::get_rect() const {
 	return rect;
 }
 
+real_t CircleShape2D::get_enclosing_radius() const {
+	return radius;
+}
+
 void CircleShape2D::draw(const RID &p_to_rid, const Color &p_color) {
 
 	Vector<Vector2> points;
diff --git a/scene/resources/circle_shape_2d.h b/scene/resources/circle_shape_2d.h
index cafd291f13..8b064f4d9f 100644
--- a/scene/resources/circle_shape_2d.h
+++ b/scene/resources/circle_shape_2d.h
@@ -50,6 +50,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	CircleShape2D();
 };
diff --git a/scene/resources/concave_polygon_shape.cpp b/scene/resources/concave_polygon_shape.cpp
index 48cc08eae4..0a93f99ea3 100644
--- a/scene/resources/concave_polygon_shape.cpp
+++ b/scene/resources/concave_polygon_shape.cpp
@@ -64,6 +64,16 @@ Vector<Vector3> ConcavePolygonShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t ConcavePolygonShape::get_enclosing_radius() const {
+	PoolVector<Vector3> data = get_faces();
+	PoolVector<Vector3>::Read read = data.read();
+	real_t r = 0;
+	for (int i(0); i < data.size(); i++) {
+		r = MAX(read[i].length_squared(), r);
+	}
+	return Math::sqrt(r);
+}
+
 void ConcavePolygonShape::_update_shape() {
 	Shape::_update_shape();
 }
diff --git a/scene/resources/concave_polygon_shape.h b/scene/resources/concave_polygon_shape.h
index 5e371954d4..b4bebbd7b4 100644
--- a/scene/resources/concave_polygon_shape.h
+++ b/scene/resources/concave_polygon_shape.h
@@ -66,7 +66,8 @@ public:
 	void set_faces(const PoolVector<Vector3> &p_faces);
 	PoolVector<Vector3> get_faces() const;
 
-	Vector<Vector3> get_debug_mesh_lines();
+	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	ConcavePolygonShape();
 };
diff --git a/scene/resources/concave_polygon_shape_2d.cpp b/scene/resources/concave_polygon_shape_2d.cpp
index 640f395158..840733add3 100644
--- a/scene/resources/concave_polygon_shape_2d.cpp
+++ b/scene/resources/concave_polygon_shape_2d.cpp
@@ -94,6 +94,16 @@ Rect2 ConcavePolygonShape2D::get_rect() const {
 	return rect;
 }
 
+real_t ConcavePolygonShape2D::get_enclosing_radius() const {
+	PoolVector<Vector2> data = get_segments();
+	PoolVector<Vector2>::Read read = data.read();
+	real_t r = 0;
+	for (int i(0); i < data.size(); i++) {
+		r = MAX(read[i].length_squared(), r);
+	}
+	return Math::sqrt(r);
+}
+
 void ConcavePolygonShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_segments", "segments"), &ConcavePolygonShape2D::set_segments);
diff --git a/scene/resources/concave_polygon_shape_2d.h b/scene/resources/concave_polygon_shape_2d.h
index b95ed100e7..4e47ad34b8 100644
--- a/scene/resources/concave_polygon_shape_2d.h
+++ b/scene/resources/concave_polygon_shape_2d.h
@@ -47,6 +47,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	ConcavePolygonShape2D();
 };
diff --git a/scene/resources/convex_polygon_shape.cpp b/scene/resources/convex_polygon_shape.cpp
index 1498eb4af9..21fdcc1f06 100644
--- a/scene/resources/convex_polygon_shape.cpp
+++ b/scene/resources/convex_polygon_shape.cpp
@@ -55,6 +55,16 @@ Vector<Vector3> ConvexPolygonShape::get_debug_mesh_lines() {
 	return Vector<Vector3>();
 }
 
+real_t ConvexPolygonShape::get_enclosing_radius() const {
+	PoolVector<Vector3> data = get_points();
+	PoolVector<Vector3>::Read read = data.read();
+	real_t r = 0;
+	for (int i(0); i < data.size(); i++) {
+		r = MAX(read[i].length_squared(), r);
+	}
+	return Math::sqrt(r);
+}
+
 void ConvexPolygonShape::_update_shape() {
 
 	PhysicsServer::get_singleton()->shape_set_data(get_shape(), points);
diff --git a/scene/resources/convex_polygon_shape.h b/scene/resources/convex_polygon_shape.h
index 4725d09b26..e3bf02399a 100644
--- a/scene/resources/convex_polygon_shape.h
+++ b/scene/resources/convex_polygon_shape.h
@@ -48,6 +48,7 @@ public:
 	PoolVector<Vector3> get_points() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	ConvexPolygonShape();
 };
diff --git a/scene/resources/convex_polygon_shape_2d.cpp b/scene/resources/convex_polygon_shape_2d.cpp
index c404190398..296d014cc7 100644
--- a/scene/resources/convex_polygon_shape_2d.cpp
+++ b/scene/resources/convex_polygon_shape_2d.cpp
@@ -97,6 +97,14 @@ Rect2 ConvexPolygonShape2D::get_rect() const {
 	return rect;
 }
 
+real_t ConvexPolygonShape2D::get_enclosing_radius() const {
+	real_t r = 0;
+	for (int i(0); i < get_points().size(); i++) {
+		r = MAX(get_points()[i].length_squared(), r);
+	}
+	return Math::sqrt(r);
+}
+
 ConvexPolygonShape2D::ConvexPolygonShape2D() :
 		Shape2D(Physics2DServer::get_singleton()->convex_polygon_shape_create()) {
 }
diff --git a/scene/resources/convex_polygon_shape_2d.h b/scene/resources/convex_polygon_shape_2d.h
index ed6be738ab..83c250c0ce 100644
--- a/scene/resources/convex_polygon_shape_2d.h
+++ b/scene/resources/convex_polygon_shape_2d.h
@@ -51,6 +51,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	ConvexPolygonShape2D();
 };
diff --git a/scene/resources/cylinder_shape.cpp b/scene/resources/cylinder_shape.cpp
index d9f6e4f054..b9b0d77a1b 100644
--- a/scene/resources/cylinder_shape.cpp
+++ b/scene/resources/cylinder_shape.cpp
@@ -62,6 +62,10 @@ Vector<Vector3> CylinderShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t CylinderShape::get_enclosing_radius() const {
+	return Vector2(radius, height * 0.5).length();
+}
+
 void CylinderShape::_update_shape() {
 
 	Dictionary d;
diff --git a/scene/resources/cylinder_shape.h b/scene/resources/cylinder_shape.h
index ebddd4d92f..a26fda10cd 100644
--- a/scene/resources/cylinder_shape.h
+++ b/scene/resources/cylinder_shape.h
@@ -50,6 +50,7 @@ public:
 	float get_height() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	CylinderShape();
 };
diff --git a/scene/resources/height_map_shape.cpp b/scene/resources/height_map_shape.cpp
index 2b86d658d8..48c9221e27 100644
--- a/scene/resources/height_map_shape.cpp
+++ b/scene/resources/height_map_shape.cpp
@@ -76,6 +76,10 @@ Vector<Vector3> HeightMapShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t HeightMapShape::get_enclosing_radius() const {
+	return Vector3(real_t(map_width), max_height - min_height, real_t(map_depth)).length();
+}
+
 void HeightMapShape::_update_shape() {
 
 	Dictionary d;
diff --git a/scene/resources/height_map_shape.h b/scene/resources/height_map_shape.h
index 3a976e3add..a6263f061f 100644
--- a/scene/resources/height_map_shape.h
+++ b/scene/resources/height_map_shape.h
@@ -55,6 +55,7 @@ public:
 	PoolRealArray get_map_data() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	HeightMapShape();
 };
diff --git a/scene/resources/line_shape_2d.cpp b/scene/resources/line_shape_2d.cpp
index 84da8125ea..d1bb61820b 100644
--- a/scene/resources/line_shape_2d.cpp
+++ b/scene/resources/line_shape_2d.cpp
@@ -100,6 +100,10 @@ Rect2 LineShape2D::get_rect() const {
 	return rect;
 }
 
+real_t LineShape2D::get_enclosing_radius() const {
+	return d;
+}
+
 void LineShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_normal", "normal"), &LineShape2D::set_normal);
diff --git a/scene/resources/line_shape_2d.h b/scene/resources/line_shape_2d.h
index 7babfe12f6..5bf9e85bb9 100644
--- a/scene/resources/line_shape_2d.h
+++ b/scene/resources/line_shape_2d.h
@@ -55,6 +55,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	LineShape2D();
 };
diff --git a/scene/resources/mesh_library.h b/scene/resources/mesh_library.h
index 471db74175..5f1fddae22 100644
--- a/scene/resources/mesh_library.h
+++ b/scene/resources/mesh_library.h
@@ -34,7 +34,7 @@
 #include "core/map.h"
 #include "core/resource.h"
 #include "mesh.h"
-#include "scene/3d/navigation_mesh.h"
+#include "scene/3d/navigation_mesh_instance.h"
 #include "shape.h"
 
 class MeshLibrary : public Resource {
diff --git a/scene/resources/navigation_mesh.cpp b/scene/resources/navigation_mesh.cpp
new file mode 100644
index 0000000000..e6544778bc
--- /dev/null
+++ b/scene/resources/navigation_mesh.cpp
@@ -0,0 +1,548 @@
+/*************************************************************************/
+/*  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"
+
+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;
+}
diff --git a/scene/resources/navigation_mesh.h b/scene/resources/navigation_mesh.h
new file mode 100644
index 0000000000..a2b1c62eab
--- /dev/null
+++ b/scene/resources/navigation_mesh.h
@@ -0,0 +1,195 @@
+/*************************************************************************/
+/*  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/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();
+};
+
+#endif // NAVIGATION_MESH_H
diff --git a/scene/resources/plane_shape.h b/scene/resources/plane_shape.h
index 8bea1268e5..360f9dbbe9 100644
--- a/scene/resources/plane_shape.h
+++ b/scene/resources/plane_shape.h
@@ -47,6 +47,10 @@ public:
 	Plane get_plane() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const {
+		// Should be infinite?
+		return 0;
+	}
 
 	PlaneShape();
 };
diff --git a/scene/resources/ray_shape.cpp b/scene/resources/ray_shape.cpp
index 3062e96293..1a9b7e6dd2 100644
--- a/scene/resources/ray_shape.cpp
+++ b/scene/resources/ray_shape.cpp
@@ -41,6 +41,10 @@ Vector<Vector3> RayShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t RayShape::get_enclosing_radius() const {
+	return length;
+}
+
 void RayShape::_update_shape() {
 
 	Dictionary d;
diff --git a/scene/resources/ray_shape.h b/scene/resources/ray_shape.h
index ddf9f56ea3..c89705ad7d 100644
--- a/scene/resources/ray_shape.h
+++ b/scene/resources/ray_shape.h
@@ -50,6 +50,7 @@ public:
 	bool get_slips_on_slope() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	RayShape();
 };
diff --git a/scene/resources/rectangle_shape_2d.cpp b/scene/resources/rectangle_shape_2d.cpp
index 0030e6dd4f..f8c8ffb289 100644
--- a/scene/resources/rectangle_shape_2d.cpp
+++ b/scene/resources/rectangle_shape_2d.cpp
@@ -59,6 +59,10 @@ Rect2 RectangleShape2D::get_rect() const {
 	return Rect2(-extents, extents * 2.0);
 }
 
+real_t RectangleShape2D::get_enclosing_radius() const {
+	return extents.length();
+}
+
 void RectangleShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_extents", "extents"), &RectangleShape2D::set_extents);
diff --git a/scene/resources/rectangle_shape_2d.h b/scene/resources/rectangle_shape_2d.h
index 686b421a2a..68fc539085 100644
--- a/scene/resources/rectangle_shape_2d.h
+++ b/scene/resources/rectangle_shape_2d.h
@@ -48,6 +48,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	RectangleShape2D();
 };
diff --git a/scene/resources/segment_shape_2d.cpp b/scene/resources/segment_shape_2d.cpp
index 0070de72a2..3094d0f9bd 100644
--- a/scene/resources/segment_shape_2d.cpp
+++ b/scene/resources/segment_shape_2d.cpp
@@ -82,6 +82,10 @@ Rect2 SegmentShape2D::get_rect() const {
 	return rect;
 }
 
+real_t SegmentShape2D::get_enclosing_radius() const {
+	return (a + b).length();
+}
+
 void SegmentShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_a", "a"), &SegmentShape2D::set_a);
@@ -138,6 +142,10 @@ Rect2 RayShape2D::get_rect() const {
 	return rect;
 }
 
+real_t RayShape2D::get_enclosing_radius() const {
+	return length;
+}
+
 void RayShape2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_length", "length"), &RayShape2D::set_length);
diff --git a/scene/resources/segment_shape_2d.h b/scene/resources/segment_shape_2d.h
index aaf838ebfb..ca10c24f07 100644
--- a/scene/resources/segment_shape_2d.h
+++ b/scene/resources/segment_shape_2d.h
@@ -55,6 +55,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	SegmentShape2D();
 };
@@ -79,6 +80,7 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color);
 	virtual Rect2 get_rect() const;
+	virtual real_t get_enclosing_radius() const;
 
 	RayShape2D();
 };
diff --git a/scene/resources/shape.h b/scene/resources/shape.h
index 8fc265f3bc..227a581c96 100644
--- a/scene/resources/shape.h
+++ b/scene/resources/shape.h
@@ -32,6 +32,7 @@
 #define SHAPE_H
 
 #include "core/resource.h"
+
 class ArrayMesh;
 
 class Shape : public Resource {
@@ -57,6 +58,8 @@ public:
 
 	Ref<ArrayMesh> get_debug_mesh();
 	virtual Vector<Vector3> get_debug_mesh_lines() = 0; // { return Vector<Vector3>(); }
+	/// Returns the radius of a sphere that fully enclose this shape
+	virtual real_t get_enclosing_radius() const = 0;
 
 	void add_vertices_to_array(PoolVector<Vector3> &array, const Transform &p_xform);
 
diff --git a/scene/resources/shape_2d.h b/scene/resources/shape_2d.h
index 13ad7492ae..e2933ec031 100644
--- a/scene/resources/shape_2d.h
+++ b/scene/resources/shape_2d.h
@@ -58,6 +58,8 @@ public:
 
 	virtual void draw(const RID &p_to_rid, const Color &p_color) {}
 	virtual Rect2 get_rect() const { return Rect2(); }
+	/// Returns the radius of a circle that fully enclose this shape
+	virtual real_t get_enclosing_radius() const = 0;
 	virtual RID get_rid() const;
 	Shape2D();
 	~Shape2D();
diff --git a/scene/resources/sphere_shape.cpp b/scene/resources/sphere_shape.cpp
index f408717834..56121b6941 100644
--- a/scene/resources/sphere_shape.cpp
+++ b/scene/resources/sphere_shape.cpp
@@ -55,6 +55,10 @@ Vector<Vector3> SphereShape::get_debug_mesh_lines() {
 	return points;
 }
 
+real_t SphereShape::get_enclosing_radius() const {
+	return radius;
+}
+
 void SphereShape::_update_shape() {
 
 	PhysicsServer::get_singleton()->shape_set_data(get_shape(), radius);
diff --git a/scene/resources/sphere_shape.h b/scene/resources/sphere_shape.h
index 6002a3baeb..07e8f1e233 100644
--- a/scene/resources/sphere_shape.h
+++ b/scene/resources/sphere_shape.h
@@ -48,6 +48,7 @@ public:
 	float get_radius() const;
 
 	virtual Vector<Vector3> get_debug_mesh_lines();
+	virtual real_t get_enclosing_radius() const;
 
 	SphereShape();
 };
diff --git a/scene/resources/world.cpp b/scene/resources/world.cpp
index 1099852098..e1e3974016 100644
--- a/scene/resources/world.cpp
+++ b/scene/resources/world.cpp
@@ -262,6 +262,7 @@ RID World::get_space() const {
 
 	return space;
 }
+
 RID World::get_scenario() const {
 
 	return scenario;