Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add Navigation Debug Visuals for GridMap cell edge connections #64173

Merged
merged 1 commit into from
Sep 1, 2022
Merged

Add Navigation Debug Visuals for GridMap cell edge connections #64173

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
179 changes: 178 additions & 1 deletion modules/gridmap/grid_map.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
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
Expand All @@ -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) {
Expand Down Expand Up @@ -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
}
}

Expand Down Expand Up @@ -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) {
Expand All @@ -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++) {
Expand All @@ -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) {
Expand Down Expand Up @@ -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 changes: 9 additions & 0 deletions modules/gridmap/grid_map.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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();
Expand Down