asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add Navigation Debug for GridMap edge connections

Browse source 
Adds navigation visual debug for GridMap edge connections that use baked navigationmesh with bake_navigation=true.
This commit is contained in:
smix8 2022-08-09 20:13:47 +02:00
parent c2eaaef149
commit 4f0730d4bc
2 changed files with 187 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

177
modules/gridmap/grid_map.cpp
View file

@ -569,7 +569,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);
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 +594,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 +694,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 +734,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 +772,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 +808,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) {
@ -1159,12 +1212,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

9
modules/gridmap/grid_map.h
View file

@ -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();