[3.x] Consider gridmap collisions in navigation bake

modules/gridmap/doc_classes/GridMap.xml

@@ -71,7 +71,7 @@
 				Returns an individual bit on the [member collision_mask].
 			</description>
 		</method>
-		<method name="get_meshes">
+		<method name="get_meshes" qualifiers="const">
 			<return type="Array" />
 			<description>
 				Returns an array of [Transform] and [Mesh] references corresponding to the non-empty cells in the grid. The transforms are specified in world space.

modules/gridmap/grid_map.cpp

@@ -35,6 +35,7 @@
 #include "scene/3d/light.h"
 #include "scene/resources/mesh_library.h"
 #include "scene/resources/physics_material.h"
+#include "scene/resources/primitive_meshes.h"
 #include "scene/resources/surface_tool.h"
 #include "scene/scene_string_names.h"
 #include "servers/navigation_server.h"
@@ -193,6 +194,24 @@ Ref<PhysicsMaterial> GridMap::get_physics_material() const {
 	return physics_material;
 }
 
+Array GridMap::get_collision_shapes() const {
+	Array shapes;
+	for (Map<OctantKey, Octant *>::Element *E = octant_map.front(); E; E = E->next()) {
+		Octant *g = E->get();
+		RID body = g->static_body;
+		Transform body_xform = PhysicsServer::get_singleton()->body_get_state(body, PhysicsServer::BODY_STATE_TRANSFORM);
+		int nshapes = PhysicsServer::get_singleton()->body_get_shape_count(body);
+		for (int i = 0; i < nshapes; i++) {
+			RID shape = PhysicsServer::get_singleton()->body_get_shape(body, i);
+			Transform xform = PhysicsServer::get_singleton()->body_get_shape_transform(body, i);
+			shapes.push_back(body_xform * xform);
+			shapes.push_back(shape);
+		}
+	}
+
+	return shapes;
+}
+
 void GridMap::set_mesh_library(const Ref<MeshLibrary> &p_mesh_library) {
 	if (!mesh_library.is_null()) {
 		mesh_library->unregister_owner(this);
@@ -939,7 +958,7 @@ Array GridMap::get_used_cells() const {
 	return a;
 }
 
-Array GridMap::get_meshes() {
+Array GridMap::get_meshes() const {
 	if (mesh_library.is_null()) {
 		return Array();
 	}

modules/gridmap/grid_map.h

@@ -225,6 +225,8 @@ class GridMap : public Spatial {
 	void set_physics_material(Ref<PhysicsMaterial> p_material);
 	Ref<PhysicsMaterial> get_physics_material() const;
 
+	Array get_collision_shapes() const;
+
 	void set_mesh_library(const Ref<MeshLibrary> &p_mesh_library);
 	Ref<MeshLibrary> get_mesh_library() const;
 
@@ -258,7 +260,7 @@ class GridMap : public Spatial {
 
 	Array get_used_cells() const;
 
-	Array get_meshes();
+	Array get_meshes() const;
 
 	void clear_baked_meshes();
 	void make_baked_meshes(bool p_gen_lightmap_uv = false, float p_lightmap_uv_texel_size = 0.1);

modules/navigation/navigation_mesh_generator.cpp

@@ -266,14 +266,100 @@ void NavigationMeshGenerator::_parse_geometry(Transform p_accumulated_transform,
 	}
 
 #ifdef MODULE_GRIDMAP_ENABLED
-	if (Object::cast_to<GridMap>(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) {
-		GridMap *gridmap_instance = Object::cast_to<GridMap>(p_node);
-		Array meshes = gridmap_instance->get_meshes();
-		Transform xform = gridmap_instance->get_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 * meshes[i], p_vertices, p_indices);
+	GridMap *gridmap = Object::cast_to<GridMap>(p_node);
+
+	if (gridmap) {
+		if (p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) {
+			Array meshes = gridmap->get_meshes();
+			Transform xform = gridmap->get_transform();
+			for (int i = 0; i < meshes.size(); i += 2) {
+				Ref<Mesh> mesh = meshes[i + 1];
+				if (mesh.is_valid()) {
+					Transform mesh_xform = meshes[i];
+					_add_mesh(mesh, p_accumulated_transform * xform * mesh_xform, p_vertices, p_indices);
+				}
+			}
+		}
+
+		if (p_generate_from != NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES && (gridmap->get_collision_layer() & p_collision_mask)) {
+			Array shapes = gridmap->get_collision_shapes();
+			for (int i = 0; i < shapes.size(); i += 2) {
+				RID shape = shapes[i + 1];
+				PhysicsServer::ShapeType type = PhysicsServer::get_singleton()->shape_get_type(shape);
+				Variant data = PhysicsServer::get_singleton()->shape_get_data(shape);
+				Ref<Mesh> mesh;
+
+				switch (type) {
+					case PhysicsServer::SHAPE_SPHERE: {
+						real_t radius = data;
+						Ref<SphereMesh> sphere_mesh;
+						sphere_mesh.instance();
+						sphere_mesh->set_radius(radius);
+						sphere_mesh->set_height(radius * 2.0);
+						mesh = sphere_mesh;
+					} break;
+					case PhysicsServer::SHAPE_BOX: {
+						Vector3 extents = data;
+						Ref<CubeMesh> box_mesh;
+						box_mesh.instance();
+						box_mesh->set_size(2.0 * extents);
+						mesh = box_mesh;
+					} break;
+					case PhysicsServer::SHAPE_CAPSULE: {
+						Dictionary dict = data;
+						real_t radius = dict["radius"];
+						real_t height = dict["height"];
+						Ref<CapsuleMesh> capsule_mesh;
+						capsule_mesh.instance();
+						capsule_mesh->set_radius(radius);
+						capsule_mesh->set_mid_height(0.5 * height);
+						mesh = capsule_mesh;
+					} break;
+					case PhysicsServer::SHAPE_CYLINDER: {
+						Dictionary dict = data;
+						real_t radius = dict["radius"];
+						real_t height = dict["height"];
+						Ref<CylinderMesh> cylinder_mesh;
+						cylinder_mesh.instance();
+						cylinder_mesh->set_height(height);
+						cylinder_mesh->set_bottom_radius(radius);
+						cylinder_mesh->set_top_radius(radius);
+						mesh = cylinder_mesh;
+					} break;
+					case PhysicsServer::SHAPE_CONVEX_POLYGON: {
+						PoolVector3Array vertices = data;
+						Geometry::MeshData md;
+
+						Error err = ConvexHullComputer::convex_hull(vertices, md);
+
+						if (err == OK) {
+							PoolVector3Array faces;
+
+							for (int j = 0; j < md.faces.size(); ++j) {
+								Geometry::MeshData::Face face = md.faces[j];
+
+								for (int k = 2; k < face.indices.size(); ++k) {
+									faces.push_back(md.vertices[face.indices[0]]);
+									faces.push_back(md.vertices[face.indices[k - 1]]);
+									faces.push_back(md.vertices[face.indices[k]]);
+								}
+							}
+
+							_add_faces(faces, shapes[i], p_vertices, p_indices);
+						}
+					} break;
+					case PhysicsServer::SHAPE_CONCAVE_POLYGON: {
+						PoolVector3Array faces = data;
+						_add_faces(faces, shapes[i], p_vertices, p_indices);
+					} break;
+					default: {
+						WARN_PRINT("Unsupported collision shape type.");
+					} break;
+				}
+
+				if (mesh.is_valid()) {
+					_add_mesh(mesh, shapes[i], p_vertices, p_indices);
+				}
 			}
 		}
 	}