8 files changed, 55 insertions, 97 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
index 76c31a5f42..1151df6390 100644
--- a/modules/navigation/nav_map.cpp
+++ b/modules/navigation/nav_map.cpp
@@ -83,12 +83,9 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 			continue;
 		}
 
-		// For each point cast a face and check the distance between the origin/destination
-		for (size_t point_id = 0; point_id < p.points.size(); point_id++) {
-			const Vector3 p1 = p.points[point_id].pos;
-			const Vector3 p2 = p.points[(point_id + 1) % p.points.size()].pos;
-			const Vector3 p3 = p.points[(point_id + 2) % p.points.size()].pos;
-			const Face3 face(p1, p2, p3);
+		// For each face check the distance between the origin/destination
+		for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
+			const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
 
 			Vector3 point = face.get_closest_point_to(p_origin);
 			float distance_to_point = point.distance_to(p_origin);
@@ -214,7 +211,7 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 			end_poly = reachable_end;
 			end_d = 1e20;
 			for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
-				Face3 f(end_poly->points[point_id - 2].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
+				Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
 				Vector3 spoint = f.get_closest_point_to(p_destination);
 				float dpoint = spoint.distance_to(p_destination);
 				if (dpoint < end_d) {
@@ -370,13 +367,12 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
 	Vector3 closest_point;
 	real_t closest_point_d = 1e20;
 
-	// Find the initial poly and the end poly on this map.
 	for (size_t i(0); i < polygons.size(); i++) {
 		const gd::Polygon &p = polygons[i];
 
-		// For each point cast a face and check the distance to the segment
+		// For each face check the distance to the segment
 		for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
-			const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+			const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
 			Vector3 inters;
 			if (f.intersects_segment(p_from, p_to, &inters)) {
 				const real_t d = closest_point_d = p_from.distance_to(inters);
@@ -416,82 +412,42 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
 }
 
 Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
-	// TODO this is really not optimal, please redesign the API to directly return all this data
-
-	Vector3 closest_point;
-	real_t closest_point_d = 1e20;
-
-	// Find the initial poly and the end poly on this map.
-	for (size_t i(0); i < polygons.size(); i++) {
-		const gd::Polygon &p = polygons[i];
-
-		// For each point cast a face and check the distance to the point
-		for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
-			const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
-			const Vector3 inters = f.get_closest_point_to(p_point);
-			const real_t d = inters.distance_to(p_point);
-			if (d < closest_point_d) {
-				closest_point = inters;
-				closest_point_d = d;
-			}
-		}
-	}
-
-	return closest_point;
+	gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
+	return cp.point;
 }
 
 Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
-	// TODO this is really not optimal, please redesign the API to directly return all this data
-
-	Vector3 closest_point;
-	Vector3 closest_point_normal;
-	real_t closest_point_d = 1e20;
-
-	// Find the initial poly and the end poly on this map.
-	for (size_t i(0); i < polygons.size(); i++) {
-		const gd::Polygon &p = polygons[i];
-
-		// For each point cast a face and check the distance to the point
-		for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
-			const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
-			const Vector3 inters = f.get_closest_point_to(p_point);
-			const real_t d = inters.distance_to(p_point);
-			if (d < closest_point_d) {
-				closest_point = inters;
-				closest_point_normal = f.get_plane().normal;
-				closest_point_d = d;
-			}
-		}
-	}
-
-	return closest_point_normal;
+	gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
+	return cp.normal;
 }
 
 RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
-	// TODO this is really not optimal, please redesign the API to directly return all this data
+	gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
+	return cp.owner;
+}
 
-	Vector3 closest_point;
-	RID closest_point_owner;
-	real_t closest_point_d = 1e20;
+gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
+	gd::ClosestPointQueryResult result;
+	real_t closest_point_ds = 1e20;
 
-	// Find the initial poly and the end poly on this map.
 	for (size_t i(0); i < polygons.size(); i++) {
 		const gd::Polygon &p = polygons[i];
 
-		// For each point cast a face and check the distance to the point
+		// For each face check the distance to the point
 		for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
-			const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+			const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
 			const Vector3 inters = f.get_closest_point_to(p_point);
-			const real_t d = inters.distance_to(p_point);
-			if (d < closest_point_d) {
-				closest_point = inters;
-				closest_point_owner = p.owner->get_self();
-				closest_point_d = d;
+			const real_t ds = inters.distance_squared_to(p_point);
+			if (ds < closest_point_ds) {
+				result.point = inters;
+				result.normal = f.get_plane().normal;
+				result.owner = p.owner->get_self();
+				closest_point_ds = ds;
 			}
 		}
 	}
 
-	return closest_point_owner;
+	return result;
 }
 
 void NavMap::add_region(NavRegion *p_region) {
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
index 1802f4e907..f46297a7ce 100644
--- a/modules/navigation/nav_map.h
+++ b/modules/navigation/nav_map.h
@@ -104,6 +104,7 @@ public:
 	Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
 	Vector3 get_closest_point(const Vector3 &p_point) const;
 	Vector3 get_closest_point_normal(const Vector3 &p_point) const;
+	gd::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
 	RID get_closest_point_owner(const Vector3 &p_point) const;
 
 	void add_region(NavRegion *p_region);
diff --git a/modules/navigation/nav_utils.h b/modules/navigation/nav_utils.h
index a6f51a4698..0c02885b10 100644
--- a/modules/navigation/nav_utils.h
+++ b/modules/navigation/nav_utils.h
@@ -32,6 +32,7 @@
 #define NAV_UTILS_H
 
 #include "core/math/vector3.h"
+#include "core/templates/vector.h"
 
 #include <vector>
 
@@ -131,6 +132,12 @@ struct NavigationPoly {
 	}
 };
 
+struct ClosestPointQueryResult {
+	Vector3 point;
+	Vector3 normal;
+	RID owner;
+};
+
 } // namespace gd
 
 #endif // NAV_UTILS_H
diff --git a/modules/navigation/navigation_mesh_editor_plugin.cpp b/modules/navigation/navigation_mesh_editor_plugin.cpp
index 6db3cbc1a3..511490ba07 100644
--- a/modules/navigation/navigation_mesh_editor_plugin.cpp
+++ b/modules/navigation/navigation_mesh_editor_plugin.cpp
@@ -33,6 +33,7 @@
 
 #include "core/io/marshalls.h"
 #include "core/io/resource_saver.h"
+#include "editor/editor_node.h"
 #include "navigation_mesh_generator.h"
 #include "scene/3d/mesh_instance_3d.h"
 #include "scene/gui/box_container.h"
@@ -45,10 +46,12 @@ void NavigationMeshEditor::_node_removed(Node *p_node) {
 	}
 }
 
-void NavigationMeshEditor::_notification(int p_option) {
-	if (p_option == NOTIFICATION_ENTER_TREE) {
-		button_bake->set_icon(get_theme_icon(SNAME("Bake"), SNAME("EditorIcons")));
-		button_reset->set_icon(get_theme_icon(SNAME("Reload"), SNAME("EditorIcons")));
+void NavigationMeshEditor::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+			button_bake->set_icon(get_theme_icon(SNAME("Bake"), SNAME("EditorIcons")));
+			button_reset->set_icon(get_theme_icon(SNAME("Reload"), SNAME("EditorIcons")));
+		} break;
 	}
 }
 
@@ -139,10 +142,9 @@ void NavigationMeshEditorPlugin::make_visible(bool p_visible) {
 	}
 }
 
-NavigationMeshEditorPlugin::NavigationMeshEditorPlugin(EditorNode *p_node) {
-	editor = p_node;
+NavigationMeshEditorPlugin::NavigationMeshEditorPlugin() {
 	navigation_mesh_editor = memnew(NavigationMeshEditor);
-	editor->get_main_control()->add_child(navigation_mesh_editor);
+	EditorNode::get_singleton()->get_main_control()->add_child(navigation_mesh_editor);
 	add_control_to_container(CONTAINER_SPATIAL_EDITOR_MENU, navigation_mesh_editor->bake_hbox);
 	navigation_mesh_editor->hide();
 	navigation_mesh_editor->bake_hbox->hide();
diff --git a/modules/navigation/navigation_mesh_editor_plugin.h b/modules/navigation/navigation_mesh_editor_plugin.h
index 6517a7be5b..d581b453b3 100644
--- a/modules/navigation/navigation_mesh_editor_plugin.h
+++ b/modules/navigation/navigation_mesh_editor_plugin.h
@@ -33,7 +33,6 @@
 
 #ifdef TOOLS_ENABLED
 
-#include "editor/editor_node.h"
 #include "editor/editor_plugin.h"
 
 class NavigationRegion3D;
@@ -58,7 +57,7 @@ class NavigationMeshEditor : public Control {
 protected:
 	void _node_removed(Node *p_node);
 	static void _bind_methods();
-	void _notification(int p_option);
+	void _notification(int p_what);
 
 public:
 	void edit(NavigationRegion3D *p_nav_region);
@@ -70,7 +69,6 @@ class NavigationMeshEditorPlugin : public EditorPlugin {
 	GDCLASS(NavigationMeshEditorPlugin, EditorPlugin);
 
 	NavigationMeshEditor *navigation_mesh_editor;
-	EditorNode *editor;
 
 public:
 	virtual String get_name() const override { return "NavigationMesh"; }
@@ -79,7 +77,7 @@ public:
 	virtual bool handles(Object *p_object) const override;
 	virtual void make_visible(bool p_visible) override;
 
-	NavigationMeshEditorPlugin(EditorNode *p_node);
+	NavigationMeshEditorPlugin();
 	~NavigationMeshEditorPlugin();
 };
 
diff --git a/modules/navigation/navigation_mesh_generator.cpp b/modules/navigation/navigation_mesh_generator.cpp
index 77fe9cdd19..61c3cefc7a 100644
--- a/modules/navigation/navigation_mesh_generator.cpp
+++ b/modules/navigation/navigation_mesh_generator.cpp
@@ -50,7 +50,6 @@
 
 #ifdef TOOLS_ENABLED
 #include "editor/editor_node.h"
-#include "editor/editor_settings.h"
 #endif
 
 #include "modules/modules_enabled.gen.h" // For csg, gridmap.
@@ -140,12 +139,12 @@ void NavigationMeshGenerator::_add_faces(const PackedVector3Array &p_faces, cons
 	}
 }
 
-void NavigationMeshGenerator::_parse_geometry(Transform3D p_accumulated_transform, Node *p_node, Vector<float> &p_vertices, Vector<int> &p_indices, NavigationMesh::ParsedGeometryType p_generate_from, uint32_t p_collision_mask, bool p_recurse_children) {
+void NavigationMeshGenerator::_parse_geometry(const Transform3D &p_navmesh_transform, Node *p_node, Vector<float> &p_vertices, Vector<int> &p_indices, NavigationMesh::ParsedGeometryType p_generate_from, uint32_t p_collision_mask, bool p_recurse_children) {
 	if (Object::cast_to<MeshInstance3D>(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) {
 		MeshInstance3D *mesh_instance = Object::cast_to<MeshInstance3D>(p_node);
 		Ref<Mesh> mesh = mesh_instance->get_mesh();
 		if (mesh.is_valid()) {
-			_add_mesh(mesh, p_accumulated_transform * mesh_instance->get_transform(), p_vertices, p_indices);
+			_add_mesh(mesh, p_navmesh_transform * mesh_instance->get_global_transform(), p_vertices, p_indices);
 		}
 	}
 
@@ -159,7 +158,7 @@ void NavigationMeshGenerator::_parse_geometry(Transform3D p_accumulated_transfor
 				n = multimesh->get_instance_count();
 			}
 			for (int i = 0; i < n; i++) {
-				_add_mesh(mesh, p_accumulated_transform * multimesh->get_instance_transform(i), p_vertices, p_indices);
+				_add_mesh(mesh, p_navmesh_transform * multimesh_instance->get_global_transform() * multimesh->get_instance_transform(i), p_vertices, p_indices);
 			}
 		}
 	}
@@ -171,7 +170,7 @@ void NavigationMeshGenerator::_parse_geometry(Transform3D p_accumulated_transfor
 		if (!meshes.is_empty()) {
 			Ref<Mesh> mesh = meshes[1];
 			if (mesh.is_valid()) {
-				_add_mesh(mesh, p_accumulated_transform * csg_shape->get_transform(), p_vertices, p_indices);
+				_add_mesh(mesh, p_navmesh_transform * csg_shape->get_global_transform(), p_vertices, p_indices);
 			}
 		}
 	}
@@ -186,7 +185,7 @@ void NavigationMeshGenerator::_parse_geometry(Transform3D p_accumulated_transfor
 				if (Object::cast_to<CollisionShape3D>(child)) {
 					CollisionShape3D *col_shape = Object::cast_to<CollisionShape3D>(child);
 
-					Transform3D transform = p_accumulated_transform * static_body->get_transform() * col_shape->get_transform();
+					Transform3D transform = p_navmesh_transform * static_body->get_global_transform() * col_shape->get_transform();
 
 					Ref<Mesh> mesh;
 					Ref<Shape3D> s = col_shape->get_shape();
@@ -270,11 +269,11 @@ void NavigationMeshGenerator::_parse_geometry(Transform3D p_accumulated_transfor
 	if (gridmap) {
 		if (p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) {
 			Array meshes = gridmap->get_meshes();
-			Transform3D xform = gridmap->get_transform();
+			Transform3D xform = gridmap->get_global_transform();
 			for (int i = 0; i < meshes.size(); i += 2) {
 				Ref<Mesh> mesh = meshes[i + 1];
 				if (mesh.is_valid()) {
-					_add_mesh(mesh, p_accumulated_transform * xform * (Transform3D)meshes[i], p_vertices, p_indices);
+					_add_mesh(mesh, p_navmesh_transform * xform * (Transform3D)meshes[i], p_vertices, p_indices);
 				}
 			}
 		}
@@ -364,14 +363,9 @@ void NavigationMeshGenerator::_parse_geometry(Transform3D p_accumulated_transfor
 	}
 #endif
 
-	if (Object::cast_to<Node3D>(p_node)) {
-		Node3D *spatial = Object::cast_to<Node3D>(p_node);
-		p_accumulated_transform = p_accumulated_transform * spatial->get_transform();
-	}
-
 	if (p_recurse_children) {
 		for (int i = 0; i < p_node->get_child_count(); i++) {
-			_parse_geometry(p_accumulated_transform, p_node->get_child(i), p_vertices, p_indices, p_generate_from, p_collision_mask, p_recurse_children);
+			_parse_geometry(p_navmesh_transform, p_node->get_child(i), p_vertices, p_indices, p_generate_from, p_collision_mask, p_recurse_children);
 		}
 	}
 }
@@ -616,7 +610,7 @@ void NavigationMeshGenerator::bake(Ref<NavigationMesh> p_nav_mesh, Node *p_node)
 		p_node->get_tree()->get_nodes_in_group(p_nav_mesh->get_source_group_name(), &parse_nodes);
 	}
 
-	Transform3D navmesh_xform = Object::cast_to<Node3D>(p_node)->get_transform().affine_inverse();
+	Transform3D navmesh_xform = Object::cast_to<Node3D>(p_node)->get_global_transform().affine_inverse();
 	for (Node *E : parse_nodes) {
 		NavigationMesh::ParsedGeometryType geometry_type = p_nav_mesh->get_parsed_geometry_type();
 		uint32_t collision_mask = p_nav_mesh->get_collision_mask();
diff --git a/modules/navigation/navigation_mesh_generator.h b/modules/navigation/navigation_mesh_generator.h
index 1ffdd39aee..21f7a4941b 100644
--- a/modules/navigation/navigation_mesh_generator.h
+++ b/modules/navigation/navigation_mesh_generator.h
@@ -52,7 +52,7 @@ protected:
 	static void _add_vertex(const Vector3 &p_vec3, Vector<float> &p_vertices);
 	static void _add_mesh(const Ref<Mesh> &p_mesh, const Transform3D &p_xform, Vector<float> &p_vertices, Vector<int> &p_indices);
 	static void _add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform, Vector<float> &p_vertices, Vector<int> &p_indices);
-	static void _parse_geometry(Transform3D p_accumulated_transform, Node *p_node, Vector<float> &p_vertices, Vector<int> &p_indices, NavigationMesh::ParsedGeometryType p_generate_from, uint32_t p_collision_mask, bool p_recurse_children);
+	static void _parse_geometry(const Transform3D &p_navmesh_transform, Node *p_node, Vector<float> &p_vertices, Vector<int> &p_indices, NavigationMesh::ParsedGeometryType p_generate_from, uint32_t p_collision_mask, bool p_recurse_children);
 
 	static void _convert_detail_mesh_to_native_navigation_mesh(const rcPolyMeshDetail *p_detail_mesh, Ref<NavigationMesh> p_nav_mesh);
 	static void _build_recast_navigation_mesh(
diff --git a/modules/navigation/rvo_agent.cpp b/modules/navigation/rvo_agent.cpp
index c967d0bf98..a6a5660c0c 100644
--- a/modules/navigation/rvo_agent.cpp
+++ b/modules/navigation/rvo_agent.cpp
@@ -75,5 +75,5 @@ void RvoAgent::dispatch_callback() {
 
 	const Variant *vp[2] = { &callback.new_velocity, &callback.udata };
 	int argc = (callback.udata.get_type() == Variant::NIL) ? 1 : 2;
-	obj->call(callback.method, vp, argc, responseCallError);
+	obj->callp(callback.method, vp, argc, responseCallError);
 }