diff options
author | RĂ©mi Verschelde <remi@verschelde.fr> | 2022-09-01 17:57:58 +0200 |
---|---|---|
committer | GitHub <noreply@github.com> | 2022-09-01 17:57:58 +0200 |
commit | 9a368eca43927907ac59d9c2f21c8f6da757e654 (patch) | |
tree | c5842b9f0ea43fc942df3184507d7e45ad7d4605 /modules | |
parent | ca59f47f317d5b0378f7b218eba865da9dd5c942 (diff) | |
parent | 4f0730d4bcad43e73c39f7d77f5bef5f2215440a (diff) |
Merge pull request #64173 from smix8/navigation_debug_gridmap_edgeconnections_4.x
Diffstat (limited to 'modules')
-rw-r--r-- | modules/gridmap/grid_map.cpp | 179 | ||||
-rw-r--r-- | modules/gridmap/grid_map.h | 9 |
2 files changed, 187 insertions, 1 deletions
diff --git a/modules/gridmap/grid_map.cpp b/modules/gridmap/grid_map.cpp index f207d4a741..8486e2c58b 100644 --- a/modules/gridmap/grid_map.cpp +++ b/modules/gridmap/grid_map.cpp @@ -638,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 @@ -661,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) { @@ -755,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 } } @@ -782,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) { @@ -808,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++) { @@ -832,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) { @@ -1231,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 diff --git a/modules/gridmap/grid_map.h b/modules/gridmap/grid_map.h index 0ed4695fb9..f83ce68b09 100644 --- a/modules/gridmap/grid_map.h +++ b/modules/gridmap/grid_map.h @@ -117,6 +117,10 @@ class GridMap : public Node3D { HashSet<IndexKey> cells; RID collision_debug; RID collision_debug_instance; +#ifdef DEBUG_ENABLED + RID navigation_debug_edge_connections_instance; + Ref<ArrayMesh> navigation_debug_edge_connections_mesh; +#endif // DEBUG_ENABLED bool dirty = false; RID static_body; @@ -186,6 +190,11 @@ class GridMap : public Node3D { bool _octant_update(const OctantKey &p_key); void _octant_clean_up(const OctantKey &p_key); void _octant_transform(const OctantKey &p_key); +#ifdef DEBUG_ENABLED + void _update_octant_navigation_debug_edge_connections_mesh(const OctantKey &p_key); + void _navigation_map_changed(RID p_map); + void _update_navigation_debug_edge_connections(); +#endif // DEBUG_ENABLED bool awaiting_update = false; void _queue_octants_dirty(); |