1 files changed, 233 insertions, 140 deletions

diff --git a/modules/gridmap/grid_map.cpp b/modules/gridmap/grid_map.cpp
index 4e4f88ed6a..7d80cbef7c 100644
--- a/modules/gridmap/grid_map.cpp
+++ b/modules/gridmap/grid_map.cpp
@@ -5,8 +5,8 @@
 /*                           GODOT ENGINE                                */
 /*                      https://godotengine.org                          */
 /*************************************************************************/
-/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur.                 */
-/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md).   */
+/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur.                 */
+/* Copyright (c) 2014-2022 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       */
@@ -34,6 +34,8 @@
 #include "core/object/message_queue.h"
 #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"
@@ -101,9 +103,10 @@ bool GridMap::_get(const StringName &p_name, Variant &r_ret) const {
 		{
 			int *w = cells.ptrw();
 			int i = 0;
-			for (Map<IndexKey, Cell>::Element *E = cell_map.front(); E; E = E->next(), i++) {
-				encode_uint64(E->key().key, (uint8_t *)&w[i * 3]);
-				encode_uint32(E->get().cell, (uint8_t *)&w[i * 3 + 2]);
+			for (const KeyValue<IndexKey, Cell> &E : cell_map) {
+				encode_uint64(E.key.key, (uint8_t *)&w[i * 3]);
+				encode_uint32(E.value.cell, (uint8_t *)&w[i * 3 + 2]);
+				i++;
 			}
 		}
 
@@ -151,32 +154,67 @@ uint32_t GridMap::get_collision_mask() const {
 	return collision_mask;
 }
 
-void GridMap::set_collision_mask_bit(int p_bit, bool p_value) {
-	uint32_t mask = get_collision_mask();
+void GridMap::set_collision_layer_value(int p_layer_number, bool p_value) {
+	ERR_FAIL_COND_MSG(p_layer_number < 1, "Collision layer number must be between 1 and 32 inclusive.");
+	ERR_FAIL_COND_MSG(p_layer_number > 32, "Collision layer number must be between 1 and 32 inclusive.");
+	uint32_t collision_layer = get_collision_layer();
 	if (p_value) {
-		mask |= 1 << p_bit;
+		collision_layer |= 1 << (p_layer_number - 1);
 	} else {
-		mask &= ~(1 << p_bit);
+		collision_layer &= ~(1 << (p_layer_number - 1));
 	}
-	set_collision_mask(mask);
+	set_collision_layer(collision_layer);
 }
 
-bool GridMap::get_collision_mask_bit(int p_bit) const {
-	return get_collision_mask() & (1 << p_bit);
+bool GridMap::get_collision_layer_value(int p_layer_number) const {
+	ERR_FAIL_COND_V_MSG(p_layer_number < 1, false, "Collision layer number must be between 1 and 32 inclusive.");
+	ERR_FAIL_COND_V_MSG(p_layer_number > 32, false, "Collision layer number must be between 1 and 32 inclusive.");
+	return get_collision_layer() & (1 << (p_layer_number - 1));
 }
 
-void GridMap::set_collision_layer_bit(int p_bit, bool p_value) {
-	uint32_t mask = get_collision_layer();
+void GridMap::set_collision_mask_value(int p_layer_number, bool p_value) {
+	ERR_FAIL_COND_MSG(p_layer_number < 1, "Collision layer number must be between 1 and 32 inclusive.");
+	ERR_FAIL_COND_MSG(p_layer_number > 32, "Collision layer number must be between 1 and 32 inclusive.");
+	uint32_t mask = get_collision_mask();
 	if (p_value) {
-		mask |= 1 << p_bit;
+		mask |= 1 << (p_layer_number - 1);
 	} else {
-		mask &= ~(1 << p_bit);
+		mask &= ~(1 << (p_layer_number - 1));
 	}
-	set_collision_layer(mask);
+	set_collision_mask(mask);
 }
 
-bool GridMap::get_collision_layer_bit(int p_bit) const {
-	return get_collision_layer() & (1 << p_bit);
+void GridMap::set_physics_material(Ref<PhysicsMaterial> p_material) {
+	physics_material = p_material;
+	_recreate_octant_data();
+}
+
+Ref<PhysicsMaterial> GridMap::get_physics_material() const {
+	return physics_material;
+}
+
+bool GridMap::get_collision_mask_value(int p_layer_number) const {
+	ERR_FAIL_COND_V_MSG(p_layer_number < 1, false, "Collision layer number must be between 1 and 32 inclusive.");
+	ERR_FAIL_COND_V_MSG(p_layer_number > 32, false, "Collision layer number must be between 1 and 32 inclusive.");
+	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) {
@@ -188,15 +226,33 @@ bool GridMap::is_baking_navigation() {
 	return bake_navigation;
 }
 
-void GridMap::set_navigation_layers(uint32_t p_layers) {
-	navigation_layers = p_layers;
+void GridMap::set_navigation_layers(uint32_t p_navigation_layers) {
+	navigation_layers = p_navigation_layers;
 	_recreate_octant_data();
 }
 
-uint32_t GridMap::get_navigation_layers() {
+uint32_t GridMap::get_navigation_layers() const {
 	return navigation_layers;
 }
 
+void GridMap::set_navigation_layer_value(int p_layer_number, bool p_value) {
+	ERR_FAIL_COND_MSG(p_layer_number < 1, "Navigation layer number must be between 1 and 32 inclusive.");
+	ERR_FAIL_COND_MSG(p_layer_number > 32, "Navigation layer number must be between 1 and 32 inclusive.");
+	uint32_t _navigation_layers = get_navigation_layers();
+	if (p_value) {
+		_navigation_layers |= 1 << (p_layer_number - 1);
+	} else {
+		_navigation_layers &= ~(1 << (p_layer_number - 1));
+	}
+	set_navigation_layers(_navigation_layers);
+}
+
+bool GridMap::get_navigation_layer_value(int p_layer_number) const {
+	ERR_FAIL_COND_V_MSG(p_layer_number < 1, false, "Navigation layer number must be between 1 and 32 inclusive.");
+	ERR_FAIL_COND_V_MSG(p_layer_number > 32, false, "Navigation layer number must be between 1 and 32 inclusive.");
+	return get_navigation_layers() & (1 << (p_layer_number - 1));
+}
+
 void GridMap::set_mesh_library(const Ref<MeshLibrary> &p_mesh_library) {
 	if (!mesh_library.is_null()) {
 		mesh_library->unregister_owner(this);
@@ -217,7 +273,7 @@ void GridMap::set_cell_size(const Vector3 &p_size) {
 	ERR_FAIL_COND(p_size.x < 0.001 || p_size.y < 0.001 || p_size.z < 0.001);
 	cell_size = p_size;
 	_recreate_octant_data();
-	emit_signal("cell_size_changed", cell_size);
+	emit_signal(SNAME("cell_size_changed"), cell_size);
 }
 
 Vector3 GridMap::get_cell_size() const {
@@ -308,6 +364,10 @@ void GridMap::set_cell_item(const Vector3i &p_position, int p_item, int p_rot) {
 		PhysicsServer3D::get_singleton()->body_attach_object_instance_id(g->static_body, get_instance_id());
 		PhysicsServer3D::get_singleton()->body_set_collision_layer(g->static_body, collision_layer);
 		PhysicsServer3D::get_singleton()->body_set_collision_mask(g->static_body, collision_mask);
+		if (physics_material.is_valid()) {
+			PhysicsServer3D::get_singleton()->body_set_param(g->static_body, PhysicsServer3D::BODY_PARAM_FRICTION, physics_material->get_friction());
+			PhysicsServer3D::get_singleton()->body_set_param(g->static_body, PhysicsServer3D::BODY_PARAM_BOUNCE, physics_material->get_bounce());
+		}
 		SceneTree *st = SceneTree::get_singleton();
 
 		if (st && st->is_debugging_collisions_hint()) {
@@ -369,10 +429,7 @@ int GridMap::get_cell_item_orientation(const Vector3i &p_position) const {
 }
 
 Vector3i GridMap::world_to_map(const Vector3 &p_world_position) const {
-	Vector3 map_position = p_world_position / cell_size;
-	map_position.x = floor(map_position.x);
-	map_position.y = floor(map_position.y);
-	map_position.z = floor(map_position.z);
+	Vector3 map_position = (p_world_position / cell_size).floor();
 	return Vector3i(map_position);
 }
 
@@ -394,6 +451,18 @@ void GridMap::_octant_transform(const OctantKey &p_key) {
 		RS::get_singleton()->instance_set_transform(g.collision_debug_instance, get_global_transform());
 	}
 
+	// update transform for NavigationServer regions and navigation debugmesh instances
+	for (const KeyValue<IndexKey, Octant::NavMesh> &E : g.navmesh_ids) {
+		if (bake_navigation) {
+			if (E.value.region.is_valid()) {
+				NavigationServer3D::get_singleton()->region_set_transform(E.value.region, get_global_transform() * E.value.xform);
+			}
+			if (E.value.navmesh_debug_instance.is_valid()) {
+				RS::get_singleton()->instance_set_transform(E.value.navmesh_debug_instance, get_global_transform() * E.value.xform);
+			}
+		}
+	}
+
 	for (int i = 0; i < g.multimesh_instances.size(); i++) {
 		RS::get_singleton()->instance_set_transform(g.multimesh_instances[i].instance, get_global_transform());
 	}
@@ -415,8 +484,11 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
 	}
 
 	//erase navigation
-	for (Map<IndexKey, Octant::NavMesh>::Element *E = g.navmesh_ids.front(); E; E = E->next()) {
-		NavigationServer3D::get_singleton()->free(E->get().region);
+	for (const KeyValue<IndexKey, Octant::NavMesh> &E : g.navmesh_ids) {
+		NavigationServer3D::get_singleton()->free(E.value.region);
+		if (E.value.navmesh_debug_instance.is_valid()) {
+			RS::get_singleton()->free(E.value.navmesh_debug_instance);
+		}
 	}
 	g.navmesh_ids.clear();
 
@@ -442,20 +514,20 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
 	 * and set said multimesh bounding box to one containing all cells which have this item
 	 */
 
-	Map<int, List<Pair<Transform, IndexKey>>> multimesh_items;
+	HashMap<int, List<Pair<Transform3D, IndexKey>>> multimesh_items;
 
-	for (Set<IndexKey>::Element *E = g.cells.front(); E; E = E->next()) {
-		ERR_CONTINUE(!cell_map.has(E->get()));
-		const Cell &c = cell_map[E->get()];
+	for (const IndexKey &E : g.cells) {
+		ERR_CONTINUE(!cell_map.has(E));
+		const Cell &c = cell_map[E];
 
 		if (!mesh_library.is_valid() || !mesh_library->has_item(c.item)) {
 			continue;
 		}
 
-		Vector3 cellpos = Vector3(E->get().x, E->get().y, E->get().z);
+		Vector3 cellpos = Vector3(E.x, E.y, E.z);
 		Vector3 ofs = _get_offset();
 
-		Transform xform;
+		Transform3D xform;
 
 		xform.basis.set_orthogonal_index(c.rot);
 		xform.set_origin(cellpos * cell_size + ofs);
@@ -463,12 +535,12 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
 		if (baked_meshes.size() == 0) {
 			if (mesh_library->get_item_mesh(c.item).is_valid()) {
 				if (!multimesh_items.has(c.item)) {
-					multimesh_items[c.item] = List<Pair<Transform, IndexKey>>();
+					multimesh_items[c.item] = List<Pair<Transform3D, IndexKey>>();
 				}
 
-				Pair<Transform, IndexKey> p;
-				p.first = xform;
-				p.second = E->get();
+				Pair<Transform3D, IndexKey> p;
+				p.first = xform * mesh_library->get_item_mesh_transform(c.item);
+				p.second = E;
 				multimesh_items[c.item].push_back(p);
 			}
 		}
@@ -494,35 +566,50 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
 
 			if (bake_navigation) {
 				RID region = NavigationServer3D::get_singleton()->region_create();
-				NavigationServer3D::get_singleton()->region_set_layers(region, navigation_layers);
+				NavigationServer3D::get_singleton()->region_set_navigation_layers(region, navigation_layers);
 				NavigationServer3D::get_singleton()->region_set_navmesh(region, navmesh);
-				NavigationServer3D::get_singleton()->region_set_transform(region, get_global_transform() * mesh_library->get_item_navmesh_transform(c.item));
+				NavigationServer3D::get_singleton()->region_set_transform(region, get_global_transform() * nm.xform);
 				NavigationServer3D::get_singleton()->region_set_map(region, get_world_3d()->get_navigation_map());
 				nm.region = region;
+
+				// add navigation debugmesh visual instances if debug is enabled
+				SceneTree *st = SceneTree::get_singleton();
+				if (st && st->is_debugging_navigation_hint()) {
+					if (!nm.navmesh_debug_instance.is_valid()) {
+						RID navmesh_debug_rid = navmesh->get_debug_mesh()->get_rid();
+						nm.navmesh_debug_instance = RS::get_singleton()->instance_create();
+						RS::get_singleton()->instance_set_base(nm.navmesh_debug_instance, navmesh_debug_rid);
+						RS::get_singleton()->mesh_surface_set_material(navmesh_debug_rid, 0, st->get_debug_navigation_material()->get_rid());
+					}
+					if (is_inside_tree()) {
+						RS::get_singleton()->instance_set_scenario(nm.navmesh_debug_instance, get_world_3d()->get_scenario());
+						RS::get_singleton()->instance_set_transform(nm.navmesh_debug_instance, get_global_transform() * nm.xform);
+					}
+				}
 			}
 
-			g.navmesh_ids[E->get()] = nm;
+			g.navmesh_ids[E] = nm;
 		}
 	}
 
 	//update multimeshes, only if not baked
 	if (baked_meshes.size() == 0) {
-		for (Map<int, List<Pair<Transform, IndexKey>>>::Element *E = multimesh_items.front(); E; E = E->next()) {
+		for (const KeyValue<int, List<Pair<Transform3D, IndexKey>>> &E : multimesh_items) {
 			Octant::MultimeshInstance mmi;
 
 			RID mm = RS::get_singleton()->multimesh_create();
-			RS::get_singleton()->multimesh_allocate_data(mm, E->get().size(), RS::MULTIMESH_TRANSFORM_3D);
-			RS::get_singleton()->multimesh_set_mesh(mm, mesh_library->get_item_mesh(E->key())->get_rid());
+			RS::get_singleton()->multimesh_allocate_data(mm, E.value.size(), RS::MULTIMESH_TRANSFORM_3D);
+			RS::get_singleton()->multimesh_set_mesh(mm, mesh_library->get_item_mesh(E.key)->get_rid());
 
 			int idx = 0;
-			for (List<Pair<Transform, IndexKey>>::Element *F = E->get().front(); F; F = F->next()) {
-				RS::get_singleton()->multimesh_instance_set_transform(mm, idx, F->get().first);
+			for (const Pair<Transform3D, IndexKey> &F : E.value) {
+				RS::get_singleton()->multimesh_instance_set_transform(mm, idx, F.first);
 #ifdef TOOLS_ENABLED
 
 				Octant::MultimeshInstance::Item it;
 				it.index = idx;
-				it.transform = F->get().first;
-				it.key = F->get().second;
+				it.transform = F.first;
+				it.key = F.second;
 				mmi.items.push_back(it);
 #endif
 
@@ -562,9 +649,9 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
 }
 
 void GridMap::_reset_physic_bodies_collision_filters() {
-	for (Map<OctantKey, Octant *>::Element *E = octant_map.front(); E; E = E->next()) {
-		PhysicsServer3D::get_singleton()->body_set_collision_layer(E->get()->static_body, collision_layer);
-		PhysicsServer3D::get_singleton()->body_set_collision_mask(E->get()->static_body, collision_mask);
+	for (const KeyValue<OctantKey, Octant *> &E : octant_map) {
+		PhysicsServer3D::get_singleton()->body_set_collision_layer(E.value->static_body, collision_layer);
+		PhysicsServer3D::get_singleton()->body_set_collision_mask(E.value->static_body, collision_mask);
 	}
 }
 
@@ -585,17 +672,17 @@ void GridMap::_octant_enter_world(const OctantKey &p_key) {
 	}
 
 	if (bake_navigation && mesh_library.is_valid()) {
-		for (Map<IndexKey, Octant::NavMesh>::Element *F = g.navmesh_ids.front(); F; F = F->next()) {
-			if (cell_map.has(F->key()) && F->get().region.is_valid() == false) {
-				Ref<NavigationMesh> nm = mesh_library->get_item_navmesh(cell_map[F->key()].item);
+		for (KeyValue<IndexKey, Octant::NavMesh> &F : g.navmesh_ids) {
+			if (cell_map.has(F.key) && F.value.region.is_valid() == false) {
+				Ref<NavigationMesh> nm = mesh_library->get_item_navmesh(cell_map[F.key].item);
 				if (nm.is_valid()) {
 					RID region = NavigationServer3D::get_singleton()->region_create();
-					NavigationServer3D::get_singleton()->region_set_layers(region, navigation_layers);
+					NavigationServer3D::get_singleton()->region_set_navigation_layers(region, navigation_layers);
 					NavigationServer3D::get_singleton()->region_set_navmesh(region, nm);
-					NavigationServer3D::get_singleton()->region_set_transform(region, get_global_transform() * F->get().xform);
+					NavigationServer3D::get_singleton()->region_set_transform(region, get_global_transform() * F.value.xform);
 					NavigationServer3D::get_singleton()->region_set_map(region, get_world_3d()->get_navigation_map());
 
-					F->get().region = region;
+					F.value.region = region;
 				}
 			}
 		}
@@ -616,10 +703,14 @@ void GridMap::_octant_exit_world(const OctantKey &p_key) {
 		RS::get_singleton()->instance_set_scenario(g.multimesh_instances[i].instance, RID());
 	}
 
-	for (Map<IndexKey, Octant::NavMesh>::Element *F = g.navmesh_ids.front(); F; F = F->next()) {
-		if (F->get().region.is_valid()) {
-			NavigationServer3D::get_singleton()->free(F->get().region);
-			F->get().region = RID();
+	for (KeyValue<IndexKey, Octant::NavMesh> &F : g.navmesh_ids) {
+		if (F.value.region.is_valid()) {
+			NavigationServer3D::get_singleton()->free(F.value.region);
+			F.value.region = RID();
+		}
+		if (F.value.navmesh_debug_instance.is_valid()) {
+			RS::get_singleton()->free(F.value.navmesh_debug_instance);
+			F.value.navmesh_debug_instance = RID();
 		}
 	}
 }
@@ -638,8 +729,13 @@ void GridMap::_octant_clean_up(const OctantKey &p_key) {
 	PhysicsServer3D::get_singleton()->free(g.static_body);
 
 	// Erase navigation
-	for (Map<IndexKey, Octant::NavMesh>::Element *E = g.navmesh_ids.front(); E; E = E->next()) {
-		NavigationServer3D::get_singleton()->free(E->get().region);
+	for (const KeyValue<IndexKey, Octant::NavMesh> &E : g.navmesh_ids) {
+		if (E.value.region.is_valid()) {
+			NavigationServer3D::get_singleton()->free(E.value.region);
+		}
+		if (E.value.navmesh_debug_instance.is_valid()) {
+			RS::get_singleton()->free(E.value.navmesh_debug_instance);
+		}
 	}
 	g.navmesh_ids.clear();
 
@@ -657,24 +753,24 @@ void GridMap::_notification(int p_what) {
 		case NOTIFICATION_ENTER_WORLD: {
 			last_transform = get_global_transform();
 
-			for (Map<OctantKey, Octant *>::Element *E = octant_map.front(); E; E = E->next()) {
-				_octant_enter_world(E->key());
+			for (const KeyValue<OctantKey, Octant *> &E : octant_map) {
+				_octant_enter_world(E.key);
 			}
 
 			for (int i = 0; i < baked_meshes.size(); i++) {
 				RS::get_singleton()->instance_set_scenario(baked_meshes[i].instance, get_world_3d()->get_scenario());
 				RS::get_singleton()->instance_set_transform(baked_meshes[i].instance, get_global_transform());
 			}
-
 		} break;
+
 		case NOTIFICATION_TRANSFORM_CHANGED: {
-			Transform new_xform = get_global_transform();
+			Transform3D new_xform = get_global_transform();
 			if (new_xform == last_transform) {
 				break;
 			}
 			//update run
-			for (Map<OctantKey, Octant *>::Element *E = octant_map.front(); E; E = E->next()) {
-				_octant_transform(E->key());
+			for (const KeyValue<OctantKey, Octant *> &E : octant_map) {
+				_octant_transform(E.key);
 			}
 
 			last_transform = new_xform;
@@ -682,11 +778,11 @@ void GridMap::_notification(int p_what) {
 			for (int i = 0; i < baked_meshes.size(); i++) {
 				RS::get_singleton()->instance_set_transform(baked_meshes[i].instance, get_global_transform());
 			}
-
 		} break;
+
 		case NOTIFICATION_EXIT_WORLD: {
-			for (Map<OctantKey, Octant *>::Element *E = octant_map.front(); E; E = E->next()) {
-				_octant_exit_world(E->key());
+			for (const KeyValue<OctantKey, Octant *> &E : octant_map) {
+				_octant_exit_world(E.key);
 			}
 
 			//_queue_octants_dirty(MAP_DIRTY_INSTANCES|MAP_DIRTY_TRANSFORMS);
@@ -695,8 +791,8 @@ void GridMap::_notification(int p_what) {
 			for (int i = 0; i < baked_meshes.size(); i++) {
 				RS::get_singleton()->instance_set_scenario(baked_meshes[i].instance, RID());
 			}
-
 		} break;
+
 		case NOTIFICATION_VISIBILITY_CHANGED: {
 			_update_visibility();
 		} break;
@@ -708,8 +804,8 @@ void GridMap::_update_visibility() {
 		return;
 	}
 
-	for (Map<OctantKey, Octant *>::Element *e = octant_map.front(); e; e = e->next()) {
-		Octant *octant = e->value();
+	for (KeyValue<OctantKey, Octant *> &e : octant_map) {
+		Octant *octant = e.value;
 		for (int i = 0; i < octant->multimesh_instances.size(); i++) {
 			const Octant::MultimeshInstance &mi = octant->multimesh_instances[i];
 			RS::get_singleton()->instance_set_visible(mi.instance, is_visible_in_tree());
@@ -732,22 +828,22 @@ void GridMap::_queue_octants_dirty() {
 
 void GridMap::_recreate_octant_data() {
 	recreating_octants = true;
-	Map<IndexKey, Cell> cell_copy = cell_map;
+	HashMap<IndexKey, Cell, IndexKey> cell_copy = cell_map;
 	_clear_internal();
-	for (Map<IndexKey, Cell>::Element *E = cell_copy.front(); E; E = E->next()) {
-		set_cell_item(Vector3i(E->key()), E->get().item, E->get().rot);
+	for (const KeyValue<IndexKey, Cell> &E : cell_copy) {
+		set_cell_item(Vector3i(E.key), E.value.item, E.value.rot);
 	}
 	recreating_octants = false;
 }
 
 void GridMap::_clear_internal() {
-	for (Map<OctantKey, Octant *>::Element *E = octant_map.front(); E; E = E->next()) {
+	for (const KeyValue<OctantKey, Octant *> &E : octant_map) {
 		if (is_inside_world()) {
-			_octant_exit_world(E->key());
+			_octant_exit_world(E.key);
 		}
 
-		_octant_clean_up(E->key());
-		memdelete(E->get());
+		_octant_clean_up(E.key);
+		memdelete(E.value);
 	}
 
 	octant_map.clear();
@@ -759,7 +855,7 @@ void GridMap::clear() {
 	clear_baked_meshes();
 }
 
-void GridMap::resource_changed(const RES &p_res) {
+void GridMap::resource_changed(const Ref<Resource> &p_res) {
 	_recreate_octant_data();
 }
 
@@ -769,15 +865,16 @@ void GridMap::_update_octants_callback() {
 	}
 
 	List<OctantKey> to_delete;
-	for (Map<OctantKey, Octant *>::Element *E = octant_map.front(); E; E = E->next()) {
-		if (_octant_update(E->key())) {
-			to_delete.push_back(E->key());
+	for (const KeyValue<OctantKey, Octant *> &E : octant_map) {
+		if (_octant_update(E.key)) {
+			to_delete.push_back(E.key);
 		}
 	}
 
 	while (to_delete.front()) {
+		memdelete(octant_map[to_delete.front()->get()]);
 		octant_map.erase(to_delete.front()->get());
-		to_delete.pop_back();
+		to_delete.pop_front();
 	}
 
 	_update_visibility();
@@ -791,11 +888,14 @@ void GridMap::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_collision_mask", "mask"), &GridMap::set_collision_mask);
 	ClassDB::bind_method(D_METHOD("get_collision_mask"), &GridMap::get_collision_mask);
 
-	ClassDB::bind_method(D_METHOD("set_collision_mask_bit", "bit", "value"), &GridMap::set_collision_mask_bit);
-	ClassDB::bind_method(D_METHOD("get_collision_mask_bit", "bit"), &GridMap::get_collision_mask_bit);
+	ClassDB::bind_method(D_METHOD("set_collision_mask_value", "layer_number", "value"), &GridMap::set_collision_mask_value);
+	ClassDB::bind_method(D_METHOD("get_collision_mask_value", "layer_number"), &GridMap::get_collision_mask_value);
+
+	ClassDB::bind_method(D_METHOD("set_collision_layer_value", "layer_number", "value"), &GridMap::set_collision_layer_value);
+	ClassDB::bind_method(D_METHOD("get_collision_layer_value", "layer_number"), &GridMap::get_collision_layer_value);
 
-	ClassDB::bind_method(D_METHOD("set_collision_layer_bit", "bit", "value"), &GridMap::set_collision_layer_bit);
-	ClassDB::bind_method(D_METHOD("get_collision_layer_bit", "bit"), &GridMap::get_collision_layer_bit);
+	ClassDB::bind_method(D_METHOD("set_physics_material", "material"), &GridMap::set_physics_material);
+	ClassDB::bind_method(D_METHOD("get_physics_material"), &GridMap::get_physics_material);
 
 	ClassDB::bind_method(D_METHOD("set_bake_navigation", "bake_navigation"), &GridMap::set_bake_navigation);
 	ClassDB::bind_method(D_METHOD("is_baking_navigation"), &GridMap::is_baking_navigation);
@@ -803,6 +903,9 @@ void GridMap::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_navigation_layers", "layers"), &GridMap::set_navigation_layers);
 	ClassDB::bind_method(D_METHOD("get_navigation_layers"), &GridMap::get_navigation_layers);
 
+	ClassDB::bind_method(D_METHOD("set_navigation_layer_value", "layer_number", "value"), &GridMap::set_navigation_layer_value);
+	ClassDB::bind_method(D_METHOD("get_navigation_layer_value", "layer_number"), &GridMap::get_navigation_layer_value);
+
 	ClassDB::bind_method(D_METHOD("set_mesh_library", "mesh_library"), &GridMap::set_mesh_library);
 	ClassDB::bind_method(D_METHOD("get_mesh_library"), &GridMap::get_mesh_library);
 
@@ -832,11 +935,10 @@ void GridMap::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_center_z", "enable"), &GridMap::set_center_z);
 	ClassDB::bind_method(D_METHOD("get_center_z"), &GridMap::get_center_z);
 
-	ClassDB::bind_method(D_METHOD("set_clip", "enabled", "clipabove", "floor", "axis"), &GridMap::set_clip, DEFVAL(true), DEFVAL(0), DEFVAL(Vector3::AXIS_X));
-
 	ClassDB::bind_method(D_METHOD("clear"), &GridMap::clear);
 
 	ClassDB::bind_method(D_METHOD("get_used_cells"), &GridMap::get_used_cells);
+	ClassDB::bind_method(D_METHOD("get_used_cells_by_item", "item"), &GridMap::get_used_cells_by_item);
 
 	ClassDB::bind_method(D_METHOD("get_meshes"), &GridMap::get_meshes);
 	ClassDB::bind_method(D_METHOD("get_bake_meshes"), &GridMap::get_bake_meshes);
@@ -846,8 +948,9 @@ void GridMap::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("make_baked_meshes", "gen_lightmap_uv", "lightmap_uv_texel_size"), &GridMap::make_baked_meshes, DEFVAL(false), DEFVAL(0.1));
 
 	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "mesh_library", PROPERTY_HINT_RESOURCE_TYPE, "MeshLibrary"), "set_mesh_library", "get_mesh_library");
+	ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "physics_material", PROPERTY_HINT_RESOURCE_TYPE, "PhysicsMaterial"), "set_physics_material", "get_physics_material");
 	ADD_GROUP("Cell", "cell_");
-	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "cell_size"), "set_cell_size", "get_cell_size");
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "cell_size", PROPERTY_HINT_NONE, "suffix:m"), "set_cell_size", "get_cell_size");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "cell_octant_size", PROPERTY_HINT_RANGE, "1,1024,1"), "set_octant_size", "get_octant_size");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "cell_center_x"), "set_center_x", "get_center_x");
 	ADD_PROPERTY(PropertyInfo(Variant::BOOL, "cell_center_y"), "set_center_y", "get_center_y");
@@ -865,28 +968,6 @@ void GridMap::_bind_methods() {
 	ADD_SIGNAL(MethodInfo("cell_size_changed", PropertyInfo(Variant::VECTOR3, "cell_size")));
 }
 
-void GridMap::set_clip(bool p_enabled, bool p_clip_above, int p_floor, Vector3::Axis p_axis) {
-	if (!p_enabled && !clip) {
-		return;
-	}
-	if (clip && p_enabled && clip_floor == p_floor && p_clip_above == clip_above && p_axis == clip_axis) {
-		return;
-	}
-
-	clip = p_enabled;
-	clip_floor = p_floor;
-	clip_axis = p_axis;
-	clip_above = p_clip_above;
-
-	//make it all update
-	for (Map<OctantKey, Octant *>::Element *E = octant_map.front(); E; E = E->next()) {
-		Octant *g = E->get();
-		g->dirty = true;
-	}
-	awaiting_update = true;
-	_update_octants_callback();
-}
-
 void GridMap::set_cell_scale(float p_scale) {
 	cell_scale = p_scale;
 	_recreate_octant_data();
@@ -900,15 +981,27 @@ Array GridMap::get_used_cells() const {
 	Array a;
 	a.resize(cell_map.size());
 	int i = 0;
-	for (Map<IndexKey, Cell>::Element *E = cell_map.front(); E; E = E->next()) {
-		Vector3 p(E->key().x, E->key().y, E->key().z);
+	for (const KeyValue<IndexKey, Cell> &E : cell_map) {
+		Vector3 p(E.key.x, E.key.y, E.key.z);
 		a[i++] = p;
 	}
 
 	return a;
 }
 
-Array GridMap::get_meshes() {
+Array GridMap::get_used_cells_by_item(int p_item) const {
+	Array a;
+	for (const KeyValue<IndexKey, Cell> &E : cell_map) {
+		if (E.value.item == p_item) {
+			Vector3 p(E.key.x, E.key.y, E.key.z);
+			a.push_back(p);
+		}
+	}
+
+	return a;
+}
+
+Array GridMap::get_meshes() const {
 	if (mesh_library.is_null()) {
 		return Array();
 	}
@@ -916,8 +1009,8 @@ Array GridMap::get_meshes() {
 	Vector3 ofs = _get_offset();
 	Array meshes;
 
-	for (Map<IndexKey, Cell>::Element *E = cell_map.front(); E; E = E->next()) {
-		int id = E->get().item;
+	for (const KeyValue<IndexKey, Cell> &E : cell_map) {
+		int id = E.value.item;
 		if (!mesh_library->has_item(id)) {
 			continue;
 		}
@@ -926,18 +1019,18 @@ Array GridMap::get_meshes() {
 			continue;
 		}
 
-		IndexKey ik = E->key();
+		IndexKey ik = E.key;
 
 		Vector3 cellpos = Vector3(ik.x, ik.y, ik.z);
 
-		Transform xform;
+		Transform3D xform;
 
-		xform.basis.set_orthogonal_index(E->get().rot);
+		xform.basis.set_orthogonal_index(E.value.rot);
 
 		xform.set_origin(cellpos * cell_size + ofs);
 		xform.basis.scale(Vector3(cell_scale, cell_scale, cell_scale));
 
-		meshes.push_back(xform);
+		meshes.push_back(xform * mesh_library->get_item_mesh_transform(id));
 		meshes.push_back(mesh);
 	}
 
@@ -966,12 +1059,12 @@ void GridMap::make_baked_meshes(bool p_gen_lightmap_uv, float p_lightmap_uv_texe
 	}
 
 	//generate
-	Map<OctantKey, Map<Ref<Material>, Ref<SurfaceTool>>> surface_map;
+	HashMap<OctantKey, HashMap<Ref<Material>, Ref<SurfaceTool>>, OctantKey> surface_map;
 
-	for (Map<IndexKey, Cell>::Element *E = cell_map.front(); E; E = E->next()) {
-		IndexKey key = E->key();
+	for (KeyValue<IndexKey, Cell> &E : cell_map) {
+		IndexKey key = E.key;
 
-		int item = E->get().item;
+		int item = E.value.item;
 		if (!mesh_library->has_item(item)) {
 			continue;
 		}
@@ -984,9 +1077,9 @@ void GridMap::make_baked_meshes(bool p_gen_lightmap_uv, float p_lightmap_uv_texe
 		Vector3 cellpos = Vector3(key.x, key.y, key.z);
 		Vector3 ofs = _get_offset();
 
-		Transform xform;
+		Transform3D xform;
 
-		xform.basis.set_orthogonal_index(E->get().rot);
+		xform.basis.set_orthogonal_index(E.value.rot);
 		xform.set_origin(cellpos * cell_size + ofs);
 		xform.basis.scale(Vector3(cell_scale, cell_scale, cell_scale));
 
@@ -996,10 +1089,10 @@ void GridMap::make_baked_meshes(bool p_gen_lightmap_uv, float p_lightmap_uv_texe
 		ok.z = key.z / octant_size;
 
 		if (!surface_map.has(ok)) {
-			surface_map[ok] = Map<Ref<Material>, Ref<SurfaceTool>>();
+			surface_map[ok] = HashMap<Ref<Material>, Ref<SurfaceTool>>();
 		}
 
-		Map<Ref<Material>, Ref<SurfaceTool>> &mat_map = surface_map[ok];
+		HashMap<Ref<Material>, Ref<SurfaceTool>> &mat_map = surface_map[ok];
 
 		for (int i = 0; i < mesh->get_surface_count(); i++) {
 			if (mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) {
@@ -1009,7 +1102,7 @@ void GridMap::make_baked_meshes(bool p_gen_lightmap_uv, float p_lightmap_uv_texe
 			Ref<Material> surf_mat = mesh->surface_get_material(i);
 			if (!mat_map.has(surf_mat)) {
 				Ref<SurfaceTool> st;
-				st.instance();
+				st.instantiate();
 				st->begin(Mesh::PRIMITIVE_TRIANGLES);
 				st->set_material(surf_mat);
 				mat_map[surf_mat] = st;
@@ -1019,11 +1112,11 @@ void GridMap::make_baked_meshes(bool p_gen_lightmap_uv, float p_lightmap_uv_texe
 		}
 	}
 
-	for (Map<OctantKey, Map<Ref<Material>, Ref<SurfaceTool>>>::Element *E = surface_map.front(); E; E = E->next()) {
+	for (KeyValue<OctantKey, HashMap<Ref<Material>, Ref<SurfaceTool>>> &E : surface_map) {
 		Ref<ArrayMesh> mesh;
-		mesh.instance();
-		for (Map<Ref<Material>, Ref<SurfaceTool>>::Element *F = E->get().front(); F; F = F->next()) {
-			F->get()->commit(mesh);
+		mesh.instantiate();
+		for (KeyValue<Ref<Material>, Ref<SurfaceTool>> &F : E.value) {
+			F.value->commit(mesh);
 		}
 
 		BakedMesh bm;
@@ -1053,7 +1146,7 @@ Array GridMap::get_bake_meshes() {
 	Array arr;
 	for (int i = 0; i < baked_meshes.size(); i++) {
 		arr.push_back(baked_meshes[i].mesh);
-		arr.push_back(Transform());
+		arr.push_back(Transform3D());
 	}
 
 	return arr;