Consider gridmap collisions in navigation bake

3 files changed, 25 insertions, 4 deletions

diff --git a/modules/gridmap/doc_classes/GridMap.xml b/modules/gridmap/doc_classes/GridMap.xml
index 885817caf1..9c28421a46 100644
--- a/modules/gridmap/doc_classes/GridMap.xml
+++ b/modules/gridmap/doc_classes/GridMap.xml
@@ -67,7 +67,7 @@
 				Returns whether or not the specified layer of the [member collision_mask] is enabled, given a [code]layer_number[/code] between 1 and 32.
 			</description>
 		</method>
-		<method name="get_meshes">
+		<method name="get_meshes" qualifiers="const">
 			<return type="Array" />
 			<description>
 				Returns an array of [Transform3D] and [Mesh] references corresponding to the non-empty cells in the grid. The transforms are specified in world space.
diff --git a/modules/gridmap/grid_map.cpp b/modules/gridmap/grid_map.cpp
index a861efcbf4..67deedf839 100644
--- a/modules/gridmap/grid_map.cpp
+++ b/modules/gridmap/grid_map.cpp
@@ -35,6 +35,7 @@
 #include "scene/3d/light_3d.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_3d.h"
@@ -197,6 +198,24 @@ bool GridMap::get_collision_mask_value(int p_layer_number) const {
 	return get_collision_mask() & (1 << (p_layer_number - 1));
 }
 
+Array GridMap::get_collision_shapes() const {
+	Array shapes;
+	for (const KeyValue<OctantKey, Octant *> &E : octant_map) {
+		Octant *g = E.value;
+		RID body = g->static_body;
+		Transform3D body_xform = PhysicsServer3D::get_singleton()->body_get_state(body, PhysicsServer3D::BODY_STATE_TRANSFORM);
+		int nshapes = PhysicsServer3D::get_singleton()->body_get_shape_count(body);
+		for (int i = 0; i < nshapes; i++) {
+			RID shape = PhysicsServer3D::get_singleton()->body_get_shape(body, i);
+			Transform3D xform = PhysicsServer3D::get_singleton()->body_get_shape_transform(body, i);
+			shapes.push_back(body_xform * xform);
+			shapes.push_back(shape);
+		}
+	}
+
+	return shapes;
+}
+
 void GridMap::set_bake_navigation(bool p_bake_navigation) {
 	bake_navigation = p_bake_navigation;
 	_recreate_octant_data();
@@ -930,7 +949,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();
 	}
@@ -938,7 +957,7 @@ Array GridMap::get_meshes() {
 	Vector3 ofs = _get_offset();
 	Array meshes;
 
-	for (KeyValue<IndexKey, Cell> &E : cell_map) {
+	for (const KeyValue<IndexKey, Cell> &E : cell_map) {
 		int id = E.value.item;
 		if (!mesh_library->has_item(id)) {
 			continue;
diff --git a/modules/gridmap/grid_map.h b/modules/gridmap/grid_map.h
index 546b530148..6cdc3b178d 100644
--- a/modules/gridmap/grid_map.h
+++ b/modules/gridmap/grid_map.h
@@ -229,6 +229,8 @@ public:
 	void set_physics_material(Ref<PhysicsMaterial> p_material);
 	Ref<PhysicsMaterial> get_physics_material() const;
 
+	Array get_collision_shapes() const;
+
 	void set_bake_navigation(bool p_bake_navigation);
 	bool is_baking_navigation();
 
@@ -265,7 +267,7 @@ public:
 
 	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);