summaryrefslogtreecommitdiff
path: root/modules/gridmap/grid_map.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'modules/gridmap/grid_map.cpp')
-rw-r--r--modules/gridmap/grid_map.cpp269
1 files changed, 259 insertions, 10 deletions
diff --git a/modules/gridmap/grid_map.cpp b/modules/gridmap/grid_map.cpp
index 7d80cbef7c..8486e2c58b 100644
--- a/modules/gridmap/grid_map.cpp
+++ b/modules/gridmap/grid_map.cpp
@@ -428,6 +428,75 @@ int GridMap::get_cell_item_orientation(const Vector3i &p_position) const {
return cell_map[key].rot;
}
+static const Basis _ortho_bases[24] = {
+ Basis(1, 0, 0, 0, 1, 0, 0, 0, 1),
+ Basis(0, -1, 0, 1, 0, 0, 0, 0, 1),
+ Basis(-1, 0, 0, 0, -1, 0, 0, 0, 1),
+ Basis(0, 1, 0, -1, 0, 0, 0, 0, 1),
+ Basis(1, 0, 0, 0, 0, -1, 0, 1, 0),
+ Basis(0, 0, 1, 1, 0, 0, 0, 1, 0),
+ Basis(-1, 0, 0, 0, 0, 1, 0, 1, 0),
+ Basis(0, 0, -1, -1, 0, 0, 0, 1, 0),
+ Basis(1, 0, 0, 0, -1, 0, 0, 0, -1),
+ Basis(0, 1, 0, 1, 0, 0, 0, 0, -1),
+ Basis(-1, 0, 0, 0, 1, 0, 0, 0, -1),
+ Basis(0, -1, 0, -1, 0, 0, 0, 0, -1),
+ Basis(1, 0, 0, 0, 0, 1, 0, -1, 0),
+ Basis(0, 0, -1, 1, 0, 0, 0, -1, 0),
+ Basis(-1, 0, 0, 0, 0, -1, 0, -1, 0),
+ Basis(0, 0, 1, -1, 0, 0, 0, -1, 0),
+ Basis(0, 0, 1, 0, 1, 0, -1, 0, 0),
+ Basis(0, -1, 0, 0, 0, 1, -1, 0, 0),
+ Basis(0, 0, -1, 0, -1, 0, -1, 0, 0),
+ Basis(0, 1, 0, 0, 0, -1, -1, 0, 0),
+ Basis(0, 0, 1, 0, -1, 0, 1, 0, 0),
+ Basis(0, 1, 0, 0, 0, 1, 1, 0, 0),
+ Basis(0, 0, -1, 0, 1, 0, 1, 0, 0),
+ Basis(0, -1, 0, 0, 0, -1, 1, 0, 0)
+};
+
+Basis GridMap::get_cell_item_basis(const Vector3i &p_position) const {
+ int orientation = get_cell_item_orientation(p_position);
+
+ if (orientation == -1) {
+ return Basis();
+ }
+
+ return get_basis_with_orthogonal_index(orientation);
+}
+
+Basis GridMap::get_basis_with_orthogonal_index(int p_index) const {
+ ERR_FAIL_INDEX_V(p_index, 24, Basis());
+
+ return _ortho_bases[p_index];
+}
+
+int GridMap::get_orthogonal_index_from_basis(const Basis &p_basis) const {
+ Basis orth = p_basis;
+ for (int i = 0; i < 3; i++) {
+ for (int j = 0; j < 3; j++) {
+ real_t v = orth[i][j];
+ if (v > 0.5) {
+ v = 1.0;
+ } else if (v < -0.5) {
+ v = -1.0;
+ } else {
+ v = 0;
+ }
+
+ orth[i][j] = v;
+ }
+ }
+
+ for (int i = 0; i < 24; i++) {
+ if (_ortho_bases[i] == orth) {
+ return i;
+ }
+ }
+
+ return 0;
+}
+
Vector3i GridMap::world_to_map(const Vector3 &p_world_position) const {
Vector3 map_position = (p_world_position / cell_size).floor();
return Vector3i(map_position);
@@ -529,7 +598,7 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
Transform3D xform;
- xform.basis.set_orthogonal_index(c.rot);
+ xform.basis = _ortho_bases[c.rot];
xform.set_origin(cellpos * cell_size + ofs);
xform.basis.scale(Vector3(cell_scale, cell_scale, cell_scale));
if (baked_meshes.size() == 0) {
@@ -569,7 +638,9 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
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() * nm.xform);
- NavigationServer3D::get_singleton()->region_set_map(region, get_world_3d()->get_navigation_map());
+ if (is_inside_tree()) {
+ 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
@@ -592,6 +663,12 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
}
}
+#ifdef DEBUG_ENABLED
+ if (bake_navigation) {
+ _update_octant_navigation_debug_edge_connections_mesh(p_key);
+ }
+#endif // DEBUG_ENABLED
+
//update multimeshes, only if not baked
if (baked_meshes.size() == 0) {
for (const KeyValue<int, List<Pair<Transform3D, IndexKey>>> &E : multimesh_items) {
@@ -686,6 +763,19 @@ void GridMap::_octant_enter_world(const OctantKey &p_key) {
}
}
}
+
+#ifdef DEBUG_ENABLED
+ if (bake_navigation) {
+ if (!g.navigation_debug_edge_connections_instance.is_valid()) {
+ g.navigation_debug_edge_connections_instance = RenderingServer::get_singleton()->instance_create();
+ }
+ if (!g.navigation_debug_edge_connections_mesh.is_valid()) {
+ g.navigation_debug_edge_connections_mesh = Ref<ArrayMesh>(memnew(ArrayMesh));
+ }
+
+ _update_octant_navigation_debug_edge_connections_mesh(p_key);
+ }
+#endif // DEBUG_ENABLED
}
}
@@ -713,6 +803,18 @@ void GridMap::_octant_exit_world(const OctantKey &p_key) {
F.value.navmesh_debug_instance = RID();
}
}
+
+#ifdef DEBUG_ENABLED
+ if (bake_navigation) {
+ if (g.navigation_debug_edge_connections_instance.is_valid()) {
+ RenderingServer::get_singleton()->free(g.navigation_debug_edge_connections_instance);
+ g.navigation_debug_edge_connections_instance = RID();
+ }
+ if (g.navigation_debug_edge_connections_mesh.is_valid()) {
+ RenderingServer::get_singleton()->free(g.navigation_debug_edge_connections_mesh->get_rid());
+ }
+ }
+#endif // DEBUG_ENABLED
}
void GridMap::_octant_clean_up(const OctantKey &p_key) {
@@ -739,6 +841,18 @@ void GridMap::_octant_clean_up(const OctantKey &p_key) {
}
g.navmesh_ids.clear();
+#ifdef DEBUG_ENABLED
+ if (bake_navigation) {
+ if (g.navigation_debug_edge_connections_instance.is_valid()) {
+ RenderingServer::get_singleton()->free(g.navigation_debug_edge_connections_instance);
+ g.navigation_debug_edge_connections_instance = RID();
+ }
+ if (g.navigation_debug_edge_connections_mesh.is_valid()) {
+ RenderingServer::get_singleton()->free(g.navigation_debug_edge_connections_mesh->get_rid());
+ }
+ }
+#endif // DEBUG_ENABLED
+
//erase multimeshes
for (int i = 0; i < g.multimesh_instances.size(); i++) {
@@ -763,6 +877,14 @@ void GridMap::_notification(int p_what) {
}
} break;
+#ifdef DEBUG_ENABLED
+ case NOTIFICATION_ENTER_TREE: {
+ if (bake_navigation && NavigationServer3D::get_singleton()->get_debug_enabled()) {
+ _update_navigation_debug_edge_connections();
+ }
+ } break;
+#endif // DEBUG_ENABLED
+
case NOTIFICATION_TRANSFORM_CHANGED: {
Transform3D new_xform = get_global_transform();
if (new_xform == last_transform) {
@@ -921,6 +1043,9 @@ void GridMap::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_cell_item", "position", "item", "orientation"), &GridMap::set_cell_item, DEFVAL(0));
ClassDB::bind_method(D_METHOD("get_cell_item", "position"), &GridMap::get_cell_item);
ClassDB::bind_method(D_METHOD("get_cell_item_orientation", "position"), &GridMap::get_cell_item_orientation);
+ ClassDB::bind_method(D_METHOD("get_cell_item_basis", "position"), &GridMap::get_cell_item_basis);
+ ClassDB::bind_method(D_METHOD("get_basis_with_orthogonal_index", "index"), &GridMap::get_basis_with_orthogonal_index);
+ ClassDB::bind_method(D_METHOD("get_orthogonal_index_from_basis", "basis"), &GridMap::get_orthogonal_index_from_basis);
ClassDB::bind_method(D_METHOD("world_to_map", "world_position"), &GridMap::world_to_map);
ClassDB::bind_method(D_METHOD("map_to_world", "map_position"), &GridMap::map_to_world);
@@ -977,23 +1102,23 @@ float GridMap::get_cell_scale() const {
return cell_scale;
}
-Array GridMap::get_used_cells() const {
- Array a;
+TypedArray<Vector3i> GridMap::get_used_cells() const {
+ TypedArray<Vector3i> a;
a.resize(cell_map.size());
int i = 0;
for (const KeyValue<IndexKey, Cell> &E : cell_map) {
- Vector3 p(E.key.x, E.key.y, E.key.z);
+ Vector3i p(E.key.x, E.key.y, E.key.z);
a[i++] = p;
}
return a;
}
-Array GridMap::get_used_cells_by_item(int p_item) const {
- Array a;
+TypedArray<Vector3i> GridMap::get_used_cells_by_item(int p_item) const {
+ TypedArray<Vector3i> 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);
+ Vector3i p(E.key.x, E.key.y, E.key.z);
a.push_back(p);
}
}
@@ -1025,7 +1150,7 @@ Array GridMap::get_meshes() const {
Transform3D xform;
- xform.basis.set_orthogonal_index(E.value.rot);
+ xform.basis = _ortho_bases[E.value.rot];
xform.set_origin(cellpos * cell_size + ofs);
xform.basis.scale(Vector3(cell_scale, cell_scale, cell_scale));
@@ -1079,7 +1204,7 @@ void GridMap::make_baked_meshes(bool p_gen_lightmap_uv, float p_lightmap_uv_texe
Transform3D xform;
- xform.basis.set_orthogonal_index(E.value.rot);
+ xform.basis = _ortho_bases[E.value.rot];
xform.set_origin(cellpos * cell_size + ofs);
xform.basis.scale(Vector3(cell_scale, cell_scale, cell_scale));
@@ -1159,12 +1284,136 @@ RID GridMap::get_bake_mesh_instance(int p_idx) {
GridMap::GridMap() {
set_notify_transform(true);
+#ifdef DEBUG_ENABLED
+ NavigationServer3D::get_singleton_mut()->connect("map_changed", callable_mp(this, &GridMap::_navigation_map_changed));
+ NavigationServer3D::get_singleton_mut()->connect("navigation_debug_changed", callable_mp(this, &GridMap::_update_navigation_debug_edge_connections));
+#endif // DEBUG_ENABLED
}
+#ifdef DEBUG_ENABLED
+void GridMap::_update_navigation_debug_edge_connections() {
+ if (bake_navigation) {
+ for (const KeyValue<OctantKey, Octant *> &E : octant_map) {
+ _update_octant_navigation_debug_edge_connections_mesh(E.key);
+ }
+ }
+}
+
+void GridMap::_navigation_map_changed(RID p_map) {
+ if (bake_navigation && is_inside_tree() && p_map == get_world_3d()->get_navigation_map()) {
+ _update_navigation_debug_edge_connections();
+ }
+}
+#endif // DEBUG_ENABLED
+
GridMap::~GridMap() {
if (!mesh_library.is_null()) {
mesh_library->unregister_owner(this);
}
clear();
+#ifdef DEBUG_ENABLED
+ NavigationServer3D::get_singleton_mut()->disconnect("map_changed", callable_mp(this, &GridMap::_navigation_map_changed));
+ NavigationServer3D::get_singleton_mut()->disconnect("navigation_debug_changed", callable_mp(this, &GridMap::_update_navigation_debug_edge_connections));
+#endif // DEBUG_ENABLED
+}
+
+#ifdef DEBUG_ENABLED
+void GridMap::_update_octant_navigation_debug_edge_connections_mesh(const OctantKey &p_key) {
+ ERR_FAIL_COND(!octant_map.has(p_key));
+ Octant &g = *octant_map[p_key];
+
+ if (!NavigationServer3D::get_singleton()->get_debug_enabled()) {
+ if (g.navigation_debug_edge_connections_instance.is_valid()) {
+ RS::get_singleton()->instance_set_visible(g.navigation_debug_edge_connections_instance, false);
+ }
+ return;
+ }
+
+ if (!is_inside_tree()) {
+ return;
+ }
+
+ if (!bake_navigation) {
+ if (g.navigation_debug_edge_connections_instance.is_valid()) {
+ RS::get_singleton()->instance_set_visible(g.navigation_debug_edge_connections_instance, false);
+ }
+ return;
+ }
+
+ if (!g.navigation_debug_edge_connections_instance.is_valid()) {
+ g.navigation_debug_edge_connections_instance = RenderingServer::get_singleton()->instance_create();
+ }
+
+ if (!g.navigation_debug_edge_connections_mesh.is_valid()) {
+ g.navigation_debug_edge_connections_mesh = Ref<ArrayMesh>(memnew(ArrayMesh));
+ }
+
+ g.navigation_debug_edge_connections_mesh->clear_surfaces();
+
+ float edge_connection_margin = NavigationServer3D::get_singleton()->map_get_edge_connection_margin(get_world_3d()->get_navigation_map());
+ float half_edge_connection_margin = edge_connection_margin * 0.5;
+
+ Vector<Vector3> vertex_array;
+
+ for (KeyValue<IndexKey, Octant::NavMesh> &F : g.navmesh_ids) {
+ if (cell_map.has(F.key) && F.value.region.is_valid()) {
+ int connections_count = NavigationServer3D::get_singleton()->region_get_connections_count(F.value.region);
+ if (connections_count == 0) {
+ continue;
+ }
+
+ for (int i = 0; i < connections_count; i++) {
+ Vector3 connection_pathway_start = NavigationServer3D::get_singleton()->region_get_connection_pathway_start(F.value.region, i);
+ Vector3 connection_pathway_end = NavigationServer3D::get_singleton()->region_get_connection_pathway_end(F.value.region, i);
+
+ Vector3 direction_start_end = connection_pathway_start.direction_to(connection_pathway_end);
+ Vector3 direction_end_start = connection_pathway_end.direction_to(connection_pathway_start);
+
+ Vector3 start_right_dir = direction_start_end.cross(Vector3(0, 1, 0));
+ Vector3 start_left_dir = -start_right_dir;
+
+ Vector3 end_right_dir = direction_end_start.cross(Vector3(0, 1, 0));
+ Vector3 end_left_dir = -end_right_dir;
+
+ Vector3 left_start_pos = connection_pathway_start + (start_left_dir * half_edge_connection_margin);
+ Vector3 right_start_pos = connection_pathway_start + (start_right_dir * half_edge_connection_margin);
+ Vector3 left_end_pos = connection_pathway_end + (end_right_dir * half_edge_connection_margin);
+ Vector3 right_end_pos = connection_pathway_end + (end_left_dir * half_edge_connection_margin);
+
+ vertex_array.push_back(right_end_pos);
+ vertex_array.push_back(left_start_pos);
+ vertex_array.push_back(right_start_pos);
+
+ vertex_array.push_back(left_end_pos);
+ vertex_array.push_back(right_end_pos);
+ vertex_array.push_back(right_start_pos);
+ }
+ }
+ }
+
+ if (vertex_array.size() == 0) {
+ return;
+ }
+
+ Ref<StandardMaterial3D> edge_connections_material = NavigationServer3D::get_singleton_mut()->get_debug_navigation_edge_connections_material();
+
+ Array mesh_array;
+ mesh_array.resize(Mesh::ARRAY_MAX);
+ mesh_array[Mesh::ARRAY_VERTEX] = vertex_array;
+
+ g.navigation_debug_edge_connections_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, mesh_array);
+ g.navigation_debug_edge_connections_mesh->surface_set_material(0, edge_connections_material);
+
+ RS::get_singleton()->instance_set_base(g.navigation_debug_edge_connections_instance, g.navigation_debug_edge_connections_mesh->get_rid());
+ RS::get_singleton()->instance_set_visible(g.navigation_debug_edge_connections_instance, is_visible_in_tree());
+ if (is_inside_tree()) {
+ RS::get_singleton()->instance_set_scenario(g.navigation_debug_edge_connections_instance, get_world_3d()->get_scenario());
+ }
+
+ bool enable_edge_connections = NavigationServer3D::get_singleton()->get_debug_navigation_enable_edge_connections();
+ if (!enable_edge_connections) {
+ RS::get_singleton()->instance_set_visible(g.navigation_debug_edge_connections_instance, false);
+ }
}
+#endif // DEBUG_ENABLED
generated by cgit v1.2.3 (git 2.25.1) at 2025-10-06 02:12:21 +0000