From ac7073f58655094d08daf59e3b1b36946c5ee02b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Gilles=20Roudi=C3=A8re?= Date: Mon, 15 Mar 2021 12:45:28 +0100 Subject: [PATCH] Allow Navigation to be more flexible --- doc/classes/NavigationServer2D.xml | 40 ++ doc/classes/NavigationServer3D.xml | 40 ++ modules/gdnavigation/gd_navigation_server.cpp | 40 +- modules/gdnavigation/gd_navigation_server.h | 7 +- modules/gdnavigation/nav_map.cpp | 518 +++++++++--------- modules/gdnavigation/nav_map.h | 1 + modules/gdnavigation/nav_region.cpp | 22 + modules/gdnavigation/nav_region.h | 8 + modules/gdnavigation/nav_utils.h | 39 +- modules/gridmap/doc_classes/GridMap.xml | 6 +- scene/2d/navigation_region_2d.cpp | 70 ++- scene/2d/navigation_region_2d.h | 1 + scene/main/viewport.cpp | 2 +- scene/resources/world_2d.cpp | 2 +- servers/navigation_server_2d.cpp | 15 + servers/navigation_server_2d.h | 9 +- servers/navigation_server_3d.cpp | 7 +- servers/navigation_server_3d.h | 9 +- 18 files changed, 504 insertions(+), 332 deletions(-) diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml index 4f34a8a4242..b811607bad0 100644 --- a/doc/classes/NavigationServer2D.xml +++ b/doc/classes/NavigationServer2D.xml @@ -267,6 +267,37 @@ Creates a new region. + + + + + + + + + Returns the ending point of a connection door. [code]connection[/code] is an index between 0 and the return value of [method region_get_connections_count]. + + + + + + + + + + + Returns the starting point of a connection door. [code]connection[/code] is an index between 0 and the return value of [method region_get_connections_count]. + + + + + + + + + Returns how many connections this [code]region[/code] has with other regions in the map. + + @@ -321,6 +352,15 @@ + + + + + + Emitted when a navigation map is updated, when a region moves or is modified. + + + diff --git a/doc/classes/NavigationServer3D.xml b/doc/classes/NavigationServer3D.xml index 0653c0d27d7..6d56428933e 100644 --- a/doc/classes/NavigationServer3D.xml +++ b/doc/classes/NavigationServer3D.xml @@ -335,6 +335,37 @@ Creates a new region. + + + + + + + + + Returns the ending point of a connection door. [code]connection[/code] is an index between 0 and the return value of [method region_get_connections_count]. + + + + + + + + + + + Returns the starting point of a connection door. [code]connection[/code] is an index between 0 and the return value of [method region_get_connections_count]. + + + + + + + + + Returns how many connections this [code]region[/code] has with other regions in the map. + + @@ -398,6 +429,15 @@ + + + + + + Emitted when a navigation map is updated, when a region moves or is modified. + + + diff --git a/modules/gdnavigation/gd_navigation_server.cpp b/modules/gdnavigation/gd_navigation_server.cpp index af76d9a4ccc..39f208c7a44 100644 --- a/modules/gdnavigation/gd_navigation_server.cpp +++ b/modules/gdnavigation/gd_navigation_server.cpp @@ -145,9 +145,13 @@ COMMAND_2(map_set_active, RID, p_map, bool, p_active) { if (p_active) { if (!map_is_active(p_map)) { active_maps.push_back(map); + active_maps_update_id.push_back(map->get_map_update_id()); } } else { - active_maps.erase(map); + int map_index = active_maps.find(map); + ERR_FAIL_COND(map_index < 0); + active_maps.remove(map_index); + active_maps_update_id.remove(map_index); } } @@ -304,6 +308,27 @@ void GdNavigationServer::region_bake_navmesh(Ref r_mesh, Node *p #endif } +int GdNavigationServer::region_get_connections_count(RID p_region) const { + NavRegion *region = region_owner.getornull(p_region); + ERR_FAIL_COND_V(!region, 0); + + return region->get_connections_count(); +} + +Vector3 GdNavigationServer::region_get_connection_pathway_start(RID p_region, int p_connection_id) const { + NavRegion *region = region_owner.getornull(p_region); + ERR_FAIL_COND_V(!region, Vector3()); + + return region->get_connection_pathway_start(p_connection_id); +} + +Vector3 GdNavigationServer::region_get_connection_pathway_end(RID p_region, int p_connection_id) const { + NavRegion *region = region_owner.getornull(p_region); + ERR_FAIL_COND_V(!region, Vector3()); + + return region->get_connection_pathway_end(p_connection_id); +} + RID GdNavigationServer::agent_create() const { auto mut_this = const_cast(this); MutexLock lock(mut_this->operations_mutex); @@ -443,7 +468,9 @@ COMMAND_1(free, RID, p_object) { agents[i]->set_map(nullptr); } - active_maps.erase(map); + int map_index = active_maps.find(map); + active_maps.remove(map_index); + active_maps_update_id.remove(map_index); map_owner.free(p_object); memdelete(map); @@ -504,10 +531,17 @@ void GdNavigationServer::process(real_t p_delta_time) { // In c++ we can't be sure that this is performed in the main thread // even with mutable functions. MutexLock lock(operations_mutex); - for (int i(0); i < active_maps.size(); i++) { + for (uint32_t i(0); i < active_maps.size(); i++) { active_maps[i]->sync(); active_maps[i]->step(p_delta_time); active_maps[i]->dispatch_callbacks(); + + // Emit a signal if a map changed. + const uint32_t new_map_update_id = active_maps[i]->get_map_update_id(); + if (new_map_update_id != active_maps_update_id[i]) { + emit_signal("map_changed", active_maps[i]->get_self()); + active_maps_update_id[i] = new_map_update_id; + } } } diff --git a/modules/gdnavigation/gd_navigation_server.h b/modules/gdnavigation/gd_navigation_server.h index 8bc65eccab2..2f51f6431e4 100644 --- a/modules/gdnavigation/gd_navigation_server.h +++ b/modules/gdnavigation/gd_navigation_server.h @@ -31,6 +31,7 @@ #ifndef GD_NAVIGATION_SERVER_H #define GD_NAVIGATION_SERVER_H +#include "core/templates/local_vector.h" #include "core/templates/rid.h" #include "core/templates/rid_owner.h" #include "servers/navigation_server_3d.h" @@ -79,7 +80,8 @@ class GdNavigationServer : public NavigationServer3D { mutable RID_PtrOwner agent_owner; bool active = true; - Vector active_maps; + LocalVector active_maps; + LocalVector active_maps_update_id; public: GdNavigationServer(); @@ -114,6 +116,9 @@ public: COMMAND_2(region_set_transform, RID, p_region, Transform, p_transform); COMMAND_2(region_set_navmesh, RID, p_region, Ref, p_nav_mesh); virtual void region_bake_navmesh(Ref r_mesh, Node *p_node) const; + virtual int region_get_connections_count(RID p_region) const; + virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const; + virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const; virtual RID agent_create() const; COMMAND_2(agent_set_map, RID, p_agent, RID, p_map); diff --git a/modules/gdnavigation/nav_map.cpp b/modules/gdnavigation/nav_map.cpp index 5289975e4b6..80f367f3a6a 100644 --- a/modules/gdnavigation/nav_map.cpp +++ b/modules/gdnavigation/nav_map.cpp @@ -40,7 +40,7 @@ @author AndreaCatania */ -#define USE_ENTRY_POINT +#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a))) void NavMap::set_up(Vector3 p_up) { up = p_up; @@ -71,13 +71,13 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const { } Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers) const { + // Find the start poly and the end poly on this map. const gd::Polygon *begin_poly = nullptr; const gd::Polygon *end_poly = nullptr; Vector3 begin_point; Vector3 end_point; float begin_d = 1e20; float end_d = 1e20; - // Find the initial poly and the end poly on this map. for (size_t i(0); i < polygons.size(); i++) { const gd::Polygon &p = polygons[i]; @@ -88,31 +88,34 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p } // For each point cast a face and check the distance between the origin/destination - for (size_t point_id = 2; point_id < p.points.size(); point_id++) { - Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_origin); - float dpoint = spoint.distance_to(p_origin); - if (dpoint < begin_d) { - begin_d = dpoint; + for (size_t point_id = 0; point_id < p.points.size(); point_id++) { + const Vector3 p1 = p.points[point_id].pos; + const Vector3 p2 = p.points[(point_id + 1) % p.points.size()].pos; + const Vector3 p3 = p.points[(point_id + 2) % p.points.size()].pos; + const Face3 face(p1, p2, p3); + + Vector3 point = face.get_closest_point_to(p_origin); + float distance_to_point = point.distance_to(p_origin); + if (distance_to_point < begin_d) { + begin_d = distance_to_point; begin_poly = &p; - begin_point = spoint; + begin_point = point; } - spoint = f.get_closest_point_to(p_destination); - dpoint = spoint.distance_to(p_destination); - if (dpoint < end_d) { - end_d = dpoint; + point = face.get_closest_point_to(p_destination); + distance_to_point = point.distance_to(p_destination); + if (distance_to_point < end_d) { + end_d = distance_to_point; end_poly = &p; - end_point = spoint; + end_point = point; } } } + // Check for trival cases if (!begin_poly || !end_poly) { - // No path return Vector(); } - if (begin_poly == end_poly) { Vector path; path.resize(2); @@ -121,95 +124,89 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p return path; } + // List of all reachable navigation polys. std::vector navigation_polys; navigation_polys.reserve(polygons.size() * 0.75); - // The elements indices in the `navigation_polys`. - int least_cost_id(-1); - List open_list; + // Add the start polygon to the reachable navigation polygons. + gd::NavigationPoly begin_navigation_poly = gd::NavigationPoly(begin_poly); + begin_navigation_poly.self_id = 0; + begin_navigation_poly.entry = begin_point; + begin_navigation_poly.back_navigation_edge_pathway_start = begin_point; + begin_navigation_poly.back_navigation_edge_pathway_end = begin_point; + navigation_polys.push_back(begin_navigation_poly); + + // List of polygon IDs to visit. + List to_visit; + to_visit.push_back(0); + + // This is an implementation of the A* algorithm. + int least_cost_id = 0; bool found_route = false; - navigation_polys.push_back(gd::NavigationPoly(begin_poly)); - { - least_cost_id = 0; - gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id]; - least_cost_poly->self_id = least_cost_id; - least_cost_poly->entry = begin_point; - } - - open_list.push_back(0); - const gd::Polygon *reachable_end = nullptr; float reachable_d = 1e30; bool is_reachable = true; - while (found_route == false) { - { - // Takes the current least_cost_poly neighbors and compute the traveled_distance of each - for (size_t i = 0; i < navigation_polys[least_cost_id].poly->edges.size(); i++) { - gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id]; + while (true) { + gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id]; - // Only consider the polygon if it in a region with compatible layers. - if ((p_layers & least_cost_poly->poly->owner->get_layers()) == 0) { + // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance. + for (size_t i = 0; i < least_cost_poly->poly->edges.size(); i++) { + const gd::Edge &edge = least_cost_poly->poly->edges[i]; + + // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon. + for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) { + const gd::Edge::Connection &connection = edge.connections[connection_index]; + + // Only consider the connection to another polygon if this polygon is in a region with compatible layers. + if ((p_layers & connection.polygon->owner->get_layers()) == 0) { continue; } - const gd::Edge &edge = least_cost_poly->poly->edges[i]; - if (!edge.other_polygon) { - continue; - } - -#ifdef USE_ENTRY_POINT - Vector3 edge_line[2] = { - least_cost_poly->poly->points[i].pos, - least_cost_poly->poly->points[(i + 1) % least_cost_poly->poly->points.size()].pos - }; - - const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly->entry, edge_line); + Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end }; + const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly->entry, pathway); const float new_distance = least_cost_poly->entry.distance_to(new_entry) + least_cost_poly->traveled_distance; -#else - const float new_distance = least_cost_poly->poly->center.distance_to(edge.other_polygon->center) + least_cost_poly->traveled_distance; -#endif auto it = std::find( navigation_polys.begin(), navigation_polys.end(), - gd::NavigationPoly(edge.other_polygon)); + gd::NavigationPoly(connection.polygon)); if (it != navigation_polys.end()) { - // Oh this was visited already, can we win the cost? - if (it->traveled_distance > new_distance) { - it->prev_navigation_poly_id = least_cost_id; - it->back_navigation_edge = edge.other_edge; + // Polygon already visited, check if we can reduce the travel cost. + if (new_distance < it->traveled_distance) { + it->back_navigation_poly_id = least_cost_id; + it->back_navigation_edge = connection.edge; + it->back_navigation_edge_pathway_start = connection.pathway_start; + it->back_navigation_edge_pathway_end = connection.pathway_end; it->traveled_distance = new_distance; -#ifdef USE_ENTRY_POINT it->entry = new_entry; -#endif } } else { - // Add to open neighbours + // Add the neighbour polygon to the reachable ones. + gd::NavigationPoly new_navigation_poly = gd::NavigationPoly(connection.polygon); + new_navigation_poly.self_id = navigation_polys.size(); + new_navigation_poly.back_navigation_poly_id = least_cost_id; + new_navigation_poly.back_navigation_edge = connection.edge; + new_navigation_poly.back_navigation_edge_pathway_start = connection.pathway_start; + new_navigation_poly.back_navigation_edge_pathway_end = connection.pathway_end; + new_navigation_poly.traveled_distance = new_distance; + new_navigation_poly.entry = new_entry; + navigation_polys.push_back(new_navigation_poly); - navigation_polys.push_back(gd::NavigationPoly(edge.other_polygon)); - gd::NavigationPoly *np = &navigation_polys[navigation_polys.size() - 1]; - - np->self_id = navigation_polys.size() - 1; - np->prev_navigation_poly_id = least_cost_id; - np->back_navigation_edge = edge.other_edge; - np->traveled_distance = new_distance; -#ifdef USE_ENTRY_POINT - np->entry = new_entry; -#endif - open_list.push_back(navigation_polys.size() - 1); + // Add the neighbour polygon to the polygons to visit. + to_visit.push_back(navigation_polys.size() - 1); } } } - // Removes the least cost polygon from the open list so we can advance. - open_list.erase(least_cost_id); + // Removes the least cost polygon from the list of polygons to visit so we can advance. + to_visit.erase(least_cost_id); - if (open_list.size() == 0) { - // When the open list is empty at this point the End Polygon is not reachable - // so use the further reachable polygon + // When the list of polygons to visit is empty at this point it means the End Polygon is not reachable + if (to_visit.size() == 0) { + // Thus use the further reachable polygon ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons"); is_reachable = false; if (reachable_end == nullptr) { @@ -234,26 +231,21 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p gd::NavigationPoly np = navigation_polys[0]; navigation_polys.clear(); navigation_polys.push_back(np); - open_list.clear(); - open_list.push_back(0); + to_visit.clear(); + to_visit.push_back(0); reachable_end = nullptr; continue; } - // Now take the new least_cost_poly from the open list. + // Find the polygon with the minimum cost from the list of polygons to visit. least_cost_id = -1; float least_cost = 1e30; - - for (auto element = open_list.front(); element != nullptr; element = element->next()) { + for (List::Element *element = to_visit.front(); element != nullptr; element = element->next()) { gd::NavigationPoly *np = &navigation_polys[element->get()]; float cost = np->traveled_distance; -#ifdef USE_ENTRY_POINT cost += np->entry.distance_to(end_point); -#else - cost += np->poly->center.distance_to(end_point); -#endif if (cost < least_cost) { least_cost_id = np->self_id; least_cost = cost; @@ -273,124 +265,108 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p // Check if we reached the end if (navigation_polys[least_cost_id].poly == end_poly) { - // Yep, done!! found_route = true; break; } } - if (found_route) { - Vector path; - if (p_optimize) { - // String pulling + // If we did not find a route, return an empty path. + if (!found_route) { + return Vector(); + } - gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id]; - Vector3 apex_point = end_point; - Vector3 portal_left = apex_point; - Vector3 portal_right = apex_point; - gd::NavigationPoly *left_poly = apex_poly; - gd::NavigationPoly *right_poly = apex_poly; - gd::NavigationPoly *p = apex_poly; + Vector path; + // Optimize the path. + if (p_optimize) { + // Set the apex poly/point to the end point + gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id]; + Vector3 apex_point = end_point; - path.push_back(end_point); + gd::NavigationPoly *left_poly = apex_poly; + Vector3 left_portal = apex_point; + gd::NavigationPoly *right_poly = apex_poly; + Vector3 right_portal = apex_point; - while (p) { - Vector3 left; - Vector3 right; + gd::NavigationPoly *p = apex_poly; -#define CLOCK_TANGENT(m_a, m_b, m_c) (((m_a) - (m_c)).cross((m_a) - (m_b))) + path.push_back(end_point); - if (p->poly == begin_poly) { - left = begin_point; - right = begin_point; + while (p) { + // Set left and right points of the pathway between polygons. + Vector3 left = p->back_navigation_edge_pathway_start; + Vector3 right = p->back_navigation_edge_pathway_end; + if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(up) < 0) { + SWAP(left, right); + } + + bool skip = false; + if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(up) >= 0) { + //process + if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(up) > 0) { + left_poly = p; + left_portal = left; } else { - int prev = p->back_navigation_edge; - int prev_n = (p->back_navigation_edge + 1) % p->poly->points.size(); - left = p->poly->points[prev].pos; - right = p->poly->points[prev_n].pos; + clip_path(navigation_polys, path, apex_poly, right_portal, right_poly); - if (p->poly->clockwise) { - SWAP(left, right); - } + apex_point = right_portal; + p = right_poly; + left_poly = p; + apex_poly = p; + left_portal = apex_point; + right_portal = apex_point; + path.push_back(apex_point); + skip = true; } + } - bool skip = false; - - if (CLOCK_TANGENT(apex_point, portal_left, left).dot(up) >= 0) { - //process - if (portal_left == apex_point || CLOCK_TANGENT(apex_point, left, portal_right).dot(up) > 0) { - left_poly = p; - portal_left = left; - } else { - clip_path(navigation_polys, path, apex_poly, portal_right, right_poly); - - apex_point = portal_right; - p = right_poly; - left_poly = p; - apex_poly = p; - portal_left = apex_point; - portal_right = apex_point; - path.push_back(apex_point); - skip = true; - } - } - - if (!skip && CLOCK_TANGENT(apex_point, portal_right, right).dot(up) <= 0) { - //process - if (portal_right == apex_point || CLOCK_TANGENT(apex_point, right, portal_left).dot(up) < 0) { - right_poly = p; - portal_right = right; - } else { - clip_path(navigation_polys, path, apex_poly, portal_left, left_poly); - - apex_point = portal_left; - p = left_poly; - right_poly = p; - apex_poly = p; - portal_right = apex_point; - portal_left = apex_point; - path.push_back(apex_point); - } - } - - if (p->prev_navigation_poly_id != -1) { - p = &navigation_polys[p->prev_navigation_poly_id]; + if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(up) <= 0) { + //process + if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(up) < 0) { + right_poly = p; + right_portal = right; } else { - // The end - p = nullptr; + clip_path(navigation_polys, path, apex_poly, left_portal, left_poly); + + apex_point = left_portal; + p = left_poly; + right_poly = p; + apex_poly = p; + right_portal = apex_point; + left_portal = apex_point; + path.push_back(apex_point); } } - if (path[path.size() - 1] != begin_point) { - path.push_back(begin_point); + // Go to the previous polygon. + if (p->back_navigation_poly_id != -1) { + p = &navigation_polys[p->back_navigation_poly_id]; + } else { + // The end + p = nullptr; } - - path.invert(); - - } else { - path.push_back(end_point); - - // Add mid points - int np_id = least_cost_id; - while (np_id != -1) { -#ifdef USE_ENTRY_POINT - Vector3 point = navigation_polys[np_id].entry; -#else - int prev = navigation_polys[np_id].back_navigation_edge; - int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size(); - Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5; -#endif - - path.push_back(point); - np_id = navigation_polys[np_id].prev_navigation_poly_id; - } - - path.invert(); } - return path; + // If the last point is not the begin point, add it to the list. + if (path[path.size() - 1] != begin_point) { + path.push_back(begin_point); + } + + path.invert(); + + } else { + path.push_back(end_point); + + // Add mid points + int np_id = least_cost_id; + while (np_id != -1) { + path.push_back(navigation_polys[np_id].entry); + np_id = navigation_polys[np_id].back_navigation_poly_id; + } + + path.invert(); } - return Vector(); + + return path; } Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { @@ -571,6 +547,7 @@ void NavMap::remove_agent_as_controlled(RvoAgent *agent) { } void NavMap::sync() { + // Check if we need to update the links. if (regenerate_polygons) { for (size_t r(0); r < regions.size(); r++) { regions[r]->scratch_polygons(); @@ -585,27 +562,30 @@ void NavMap::sync() { } if (regenerate_links) { - // Copy all region polygons in the map. + // Remove regions connections. + for (size_t r(0); r < regions.size(); r++) { + regions[r]->get_connections().clear(); + } + + // Resize the polygon count. int count = 0; for (size_t r(0); r < regions.size(); r++) { count += regions[r]->get_polygons().size(); } - polygons.resize(count); - count = 0; + // Copy all region polygons in the map. + count = 0; for (size_t r(0); r < regions.size(); r++) { std::copy( regions[r]->get_polygons().data(), regions[r]->get_polygons().data() + regions[r]->get_polygons().size(), polygons.begin() + count); - count += regions[r]->get_polygons().size(); } - // Connects the `Edges` of all the `Polygons` of all `Regions` each other. - Map connections; - + // Group all edges per key. + Map> connections; for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) { gd::Polygon &poly(polygons[poly_id]); @@ -613,30 +593,18 @@ void NavMap::sync() { int next_point = (p + 1) % poly.points.size(); gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key); - Map::Element *connection = connections.find(ek); + Map>::Element *connection = connections.find(ek); if (!connection) { - // Nothing yet - gd::Connection c; - c.A = &poly; - c.A_edge = p; - c.B = nullptr; - c.B_edge = -1; - connections[ek] = c; - - } else if (connection->get().B == nullptr) { - CRASH_COND(connection->get().A == nullptr); // Unreachable - - // Connect the two Polygons by this edge - connection->get().B = &poly; - connection->get().B_edge = p; - - connection->get().A->edges[connection->get().A_edge].this_edge = connection->get().A_edge; - connection->get().A->edges[connection->get().A_edge].other_polygon = connection->get().B; - connection->get().A->edges[connection->get().A_edge].other_edge = connection->get().B_edge; - - connection->get().B->edges[connection->get().B_edge].this_edge = connection->get().B_edge; - connection->get().B->edges[connection->get().B_edge].other_polygon = connection->get().A; - connection->get().B->edges[connection->get().B_edge].other_edge = connection->get().A_edge; + connections[ek] = Vector(); + } + if (connections[ek].size() <= 1) { + // Add the polygon/edge tuple to this key. + gd::Edge::Connection new_connection; + new_connection.polygon = &poly; + new_connection.edge = p; + new_connection.pathway_start = poly.points[p].pos; + new_connection.pathway_end = poly.points[next_point].pos; + connections[ek].push_back(new_connection); } else { // The edge is already connected with another edge, skip. ERR_PRINT("Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the current `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem."); @@ -644,38 +612,21 @@ void NavMap::sync() { } } - // Takes all the free edges. - std::vector free_edges; - free_edges.reserve(connections.size()); - - for (auto connection_element = connections.front(); connection_element; connection_element = connection_element->next()) { - if (connection_element->get().B == nullptr) { - CRASH_COND(connection_element->get().A == nullptr); // Unreachable - CRASH_COND(connection_element->get().A_edge < 0); // Unreachable - - // This is a free edge - uint32_t id(free_edges.size()); - free_edges.push_back(gd::FreeEdge()); - free_edges[id].is_free = true; - free_edges[id].poly = connection_element->get().A; - free_edges[id].edge_id = connection_element->get().A_edge; - uint32_t point_0(free_edges[id].edge_id); - uint32_t point_1((free_edges[id].edge_id + 1) % free_edges[id].poly->points.size()); - Vector3 pos_0 = free_edges[id].poly->points[point_0].pos; - Vector3 pos_1 = free_edges[id].poly->points[point_1].pos; - Vector3 relative = pos_1 - pos_0; - free_edges[id].edge_center = (pos_0 + pos_1) / 2.0; - free_edges[id].edge_dir = relative.normalized(); - free_edges[id].edge_len_squared = relative.length_squared(); + Vector free_edges; + for (Map>::Element *E = connections.front(); E; E = E->next()) { + if (E->get().size() == 2) { + // Connect edge that are shared in different polygons. + gd::Edge::Connection &c1 = E->get().write[0]; + gd::Edge::Connection &c2 = E->get().write[1]; + c1.polygon->edges[c1.edge].connections.push_back(c2); + c2.polygon->edges[c2.edge].connections.push_back(c1); + // Note: The pathway_start/end are full for those connection and do not need to be modified. + } else { + CRASH_COND_MSG(E->get().size() != 1, vformat("Number of connection != 1. Found: %d", E->get().size())); + free_edges.push_back(E->get()[0]); } } - const float ecm_squared(edge_connection_margin * edge_connection_margin); -#define LEN_TOLLERANCE 0.1 -#define DIR_TOLLERANCE 0.9 - // In front of tolerance -#define IFO_TOLLERANCE 0.5 - // Find the compatible near edges. // // Note: @@ -683,43 +634,67 @@ void NavMap::sync() { // to be connected, create new polygons to remove that small gap is // not really useful and would result in wasteful computation during // connection, integration and path finding. - for (size_t i(0); i < free_edges.size(); i++) { - if (!free_edges[i].is_free) { - continue; - } - gd::FreeEdge &edge = free_edges[i]; - for (size_t y(0); y < free_edges.size(); y++) { - gd::FreeEdge &other_edge = free_edges[y]; - if (i == y || !other_edge.is_free || edge.poly->owner == other_edge.poly->owner) { + for (int i = 0; i < free_edges.size(); i++) { + const gd::Edge::Connection &free_edge = free_edges[i]; + Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos; + Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos; + + for (int j = 0; j < free_edges.size(); j++) { + const gd::Edge::Connection &other_edge = free_edges[j]; + if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) { continue; } - Vector3 rel_centers = other_edge.edge_center - edge.edge_center; - if (ecm_squared > rel_centers.length_squared() // Are enough closer? - && ABS(edge.edge_len_squared - other_edge.edge_len_squared) < LEN_TOLLERANCE // Are the same length? - && ABS(edge.edge_dir.dot(other_edge.edge_dir)) > DIR_TOLLERANCE // Are aligned? - && ABS(rel_centers.normalized().dot(edge.edge_dir)) < IFO_TOLLERANCE // Are one in front the other? - ) { - // The edges can be connected - edge.is_free = false; - other_edge.is_free = false; + Vector3 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos; + Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos; - edge.poly->edges[edge.edge_id].this_edge = edge.edge_id; - edge.poly->edges[edge.edge_id].other_edge = other_edge.edge_id; - edge.poly->edges[edge.edge_id].other_polygon = other_edge.poly; - - other_edge.poly->edges[other_edge.edge_id].this_edge = other_edge.edge_id; - other_edge.poly->edges[other_edge.edge_id].other_edge = edge.edge_id; - other_edge.poly->edges[other_edge.edge_id].other_polygon = edge.poly; + // Compute the projection of the opposite edge on the current one + Vector3 edge_vector = edge_p2 - edge_p1; + float projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector.length_squared()); + float projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector.length_squared()); + if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) { + continue; } + + // Check if the two edges are close to each other enough and compute a pathway between the two regions. + Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1; + Vector3 other1; + if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) { + other1 = other_edge_p1; + } else { + other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio)); + } + if ((self1 - other1).length() > edge_connection_margin) { + continue; + } + + Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1; + Vector3 other2; + if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) { + other2 = other_edge_p2; + } else { + other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio)); + } + if ((self2 - other2).length() > edge_connection_margin) { + continue; + } + + // The edges can now be connected. + gd::Edge::Connection new_connection = other_edge; + new_connection.pathway_start = (self1 + other1) / 2.0; + new_connection.pathway_end = (self2 + other2) / 2.0; + free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection); + + // Add the connection to the region_connection map. + free_edge.polygon->owner->get_connections().push_back(new_connection); } } - } - if (regenerate_links) { + // Update the update ID. map_update_id = (map_update_id + 1) % 9999999; } + // Update agents tree. if (agents_dirty) { std::vector raw_agents; raw_agents.reserve(agents.size()); @@ -771,16 +746,15 @@ void NavMap::clip_path(const std::vector &p_navigation_polys cut_plane.d = cut_plane.normal.dot(from); while (from_poly != p_to_poly) { - int back_nav_edge = from_poly->back_navigation_edge; - Vector3 a = from_poly->poly->points[back_nav_edge].pos; - Vector3 b = from_poly->poly->points[(back_nav_edge + 1) % from_poly->poly->points.size()].pos; + Vector3 pathway_start = from_poly->back_navigation_edge_pathway_start; + Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end; - ERR_FAIL_COND(from_poly->prev_navigation_poly_id == -1); - from_poly = &p_navigation_polys[from_poly->prev_navigation_poly_id]; + ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1); + from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id]; - if (a.distance_to(b) > CMP_EPSILON) { + if (pathway_start.distance_to(pathway_end) > CMP_EPSILON) { Vector3 inters; - if (cut_plane.intersects_segment(a, b, &inters)) { + if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) { if (inters.distance_to(p_to_point) > CMP_EPSILON && inters.distance_to(path[path.size() - 1]) > CMP_EPSILON) { path.push_back(inters); } diff --git a/modules/gdnavigation/nav_map.h b/modules/gdnavigation/nav_map.h index 6b7cfae3247..8e013a72eb6 100644 --- a/modules/gdnavigation/nav_map.h +++ b/modules/gdnavigation/nav_map.h @@ -34,6 +34,7 @@ #include "nav_rid.h" #include "core/math/math_defs.h" +#include "core/templates/map.h" #include "nav_utils.h" #include diff --git a/modules/gdnavigation/nav_region.cpp b/modules/gdnavigation/nav_region.cpp index a07995bc11c..c1690b2a4ba 100644 --- a/modules/gdnavigation/nav_region.cpp +++ b/modules/gdnavigation/nav_region.cpp @@ -39,6 +39,9 @@ void NavRegion::set_map(NavMap *p_map) { map = p_map; polygons_dirty = true; + if (!map) { + connections.clear(); + } } void NavRegion::set_layers(uint32_t p_layers) { @@ -59,6 +62,25 @@ void NavRegion::set_mesh(Ref p_mesh) { polygons_dirty = true; } +int NavRegion::get_connections_count() const { + if (!map) { + return 0; + } + return connections.size(); +} + +Vector3 NavRegion::get_connection_pathway_start(int p_connection_id) const { + ERR_FAIL_COND_V(!map, Vector3()); + ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3()); + return connections[p_connection_id].pathway_start; +} + +Vector3 NavRegion::get_connection_pathway_end(int p_connection_id) const { + ERR_FAIL_COND_V(!map, Vector3()); + ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3()); + return connections[p_connection_id].pathway_end; +} + bool NavRegion::sync() { bool something_changed = polygons_dirty /* || something_dirty? */; diff --git a/modules/gdnavigation/nav_region.h b/modules/gdnavigation/nav_region.h index fff7843fde6..527b2500ac6 100644 --- a/modules/gdnavigation/nav_region.h +++ b/modules/gdnavigation/nav_region.h @@ -49,6 +49,7 @@ class NavRegion : public NavRid { Transform transform; Ref mesh; uint32_t layers = 1; + Vector connections; bool polygons_dirty = true; @@ -80,6 +81,13 @@ public: return mesh; } + Vector &get_connections() { + return connections; + } + int get_connections_count() const; + Vector3 get_connection_pathway_start(int p_connection_id) const; + Vector3 get_connection_pathway_end(int p_connection_id) const; + std::vector const &get_polygons() const { return polygons; } diff --git a/modules/gdnavigation/nav_utils.h b/modules/gdnavigation/nav_utils.h index d257a95ef11..35da391eea3 100644 --- a/modules/gdnavigation/nav_utils.h +++ b/modules/gdnavigation/nav_utils.h @@ -81,11 +81,14 @@ struct Edge { /// This edge ID int this_edge = -1; - /// Other Polygon - Polygon *other_polygon = nullptr; - - /// The other `Polygon` at this edge id has this `Polygon`. - int other_edge = -1; + /// The gateway in the edge, as, in some case, the whole edge might not be navigable. + struct Connection { + Polygon *polygon = nullptr; + int edge = -1; + Vector3 pathway_start; + Vector3 pathway_end; + }; + Vector connections; }; struct Polygon { @@ -104,21 +107,17 @@ struct Polygon { Vector3 center; }; -struct Connection { - Polygon *A = nullptr; - int A_edge = -1; - Polygon *B = nullptr; - int B_edge = -1; -}; - struct NavigationPoly { uint32_t self_id = 0; /// This poly. const Polygon *poly; - /// The previous navigation poly (id in the `navigation_poly` array). - int prev_navigation_poly_id = -1; - /// The edge id in this `Poly` to reach the `prev_navigation_poly_id`. - uint32_t back_navigation_edge = 0; + + /// Those 4 variables are used to travel the path backwards. + int back_navigation_poly_id = -1; + uint32_t back_navigation_edge = UINT32_MAX; + Vector3 back_navigation_edge_pathway_start; + Vector3 back_navigation_edge_pathway_end; + /// The entry location of this poly. Vector3 entry; /// The distance to the destination. @@ -136,14 +135,6 @@ struct NavigationPoly { } }; -struct FreeEdge { - bool is_free = false; - Polygon *poly = nullptr; - uint32_t edge_id = 0; - Vector3 edge_center; - Vector3 edge_dir; - float edge_len_squared = 0.0; -}; } // namespace gd #endif // NAV_UTILS_H diff --git a/modules/gridmap/doc_classes/GridMap.xml b/modules/gridmap/doc_classes/GridMap.xml index 25776b1f6d4..9b6fa138e55 100644 --- a/modules/gridmap/doc_classes/GridMap.xml +++ b/modules/gridmap/doc_classes/GridMap.xml @@ -214,12 +214,12 @@ The physics layers this GridMap detects collisions in. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information. - - The navigation layers the GridMap generates its navigable regions in. - The assigned [MeshLibrary]. + + The navigation layers the GridMap generates its navigable regions in. + diff --git a/scene/2d/navigation_region_2d.cpp b/scene/2d/navigation_region_2d.cpp index 794993f8926..8be8c8db4ae 100644 --- a/scene/2d/navigation_region_2d.cpp +++ b/scene/2d/navigation_region_2d.cpp @@ -364,8 +364,10 @@ void NavigationRegion2D::set_enabled(bool p_enabled) { if (!enabled) { NavigationServer2D::get_singleton()->region_set_map(region, RID()); + NavigationServer2D::get_singleton()->disconnect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed)); } else { NavigationServer2D::get_singleton()->region_set_map(region, get_world_2d()->get_navigation_map()); + NavigationServer2D::get_singleton()->connect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed)); } if (Engine::get_singleton()->is_editor_hint() || get_tree()->is_debugging_navigation_hint()) { @@ -401,6 +403,7 @@ void NavigationRegion2D::_notification(int p_what) { case NOTIFICATION_ENTER_TREE: { if (enabled) { NavigationServer2D::get_singleton()->region_set_map(region, get_world_2d()->get_navigation_map()); + NavigationServer2D::get_singleton()->connect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed)); } } break; case NOTIFICATION_TRANSFORM_CHANGED: { @@ -408,12 +411,14 @@ void NavigationRegion2D::_notification(int p_what) { } break; case NOTIFICATION_EXIT_TREE: { NavigationServer2D::get_singleton()->region_set_map(region, RID()); + if (enabled) { + NavigationServer2D::get_singleton()->disconnect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed)); + } } break; case NOTIFICATION_DRAW: { if (is_inside_tree() && (Engine::get_singleton()->is_editor_hint() || get_tree()->is_debugging_navigation_hint()) && navpoly.is_valid()) { Vector verts = navpoly->get_vertices(); - int vsize = verts.size(); - if (vsize < 3) { + if (verts.size() < 3) { return; } @@ -423,33 +428,47 @@ void NavigationRegion2D::_notification(int p_what) { } else { color = get_tree()->get_debug_navigation_disabled_color(); } - Vector colors; - Vector vertices; - vertices.resize(vsize); - colors.resize(vsize); - { - const Vector2 *vr = verts.ptr(); - for (int i = 0; i < vsize; i++) { - vertices.write[i] = vr[i]; - colors.write[i] = color; - } - } + Color doors_color = color.lightened(0.2); - Vector indices; + RandomPCG rand; for (int i = 0; i < navpoly->get_polygon_count(); i++) { + // An array of vertices for this polygon. Vector polygon = navpoly->get_polygon(i); - - for (int j = 2; j < polygon.size(); j++) { - int kofs[3] = { 0, j - 1, j }; - for (int k = 0; k < 3; k++) { - int idx = polygon[kofs[k]]; - ERR_FAIL_INDEX(idx, vsize); - indices.push_back(idx); - } + Vector vertices; + vertices.resize(polygon.size()); + for (int j = 0; j < polygon.size(); j++) { + ERR_FAIL_INDEX(polygon[j], verts.size()); + vertices.write[j] = verts[polygon[j]]; } + + // Generate the polygon color, slightly randomly modified from the settings one. + Color random_variation_color; + random_variation_color.set_hsv(color.get_h() + rand.random(-1.0, 1.0) * 0.05, color.get_s(), color.get_v() + rand.random(-1.0, 1.0) * 0.1); + random_variation_color.a = color.a; + Vector colors; + colors.push_back(random_variation_color); + + RS::get_singleton()->canvas_item_add_polygon(get_canvas_item(), vertices, colors); + } + + // Draw the region + Transform2D xform = get_global_transform(); + const NavigationServer2D *ns = NavigationServer2D::get_singleton(); + float radius = ns->map_get_edge_connection_margin(get_world_2d()->get_navigation_map()) / 2.0; + for (int i = 0; i < ns->region_get_connections_count(region); i++) { + // Two main points + Vector2 a = ns->region_get_connection_pathway_start(region, i); + a = xform.affine_inverse().xform(a); + Vector2 b = ns->region_get_connection_pathway_end(region, i); + b = xform.affine_inverse().xform(b); + draw_line(a, b, doors_color); + + // Draw a circle to illustrate the margins. + float angle = (b - a).angle(); + draw_arc(a, radius, angle + Math_PI / 2.0, angle - Math_PI / 2.0 + Math_TAU, 10, doors_color); + draw_arc(b, radius, angle - Math_PI / 2.0, angle + Math_PI / 2.0, 10, doors_color); } - RS::get_singleton()->canvas_item_add_triangle_array(get_canvas_item(), indices, vertices, colors); } } break; } @@ -484,6 +503,11 @@ void NavigationRegion2D::_navpoly_changed() { update(); } } +void NavigationRegion2D::_map_changed(RID p_map) { + if (enabled && get_world_2d()->get_navigation_map() == p_map) { + update(); + } +} String NavigationRegion2D::get_configuration_warning() const { if (!is_visible_in_tree() || !is_inside_tree()) { diff --git a/scene/2d/navigation_region_2d.h b/scene/2d/navigation_region_2d.h index 7b471bd555b..58f04599be9 100644 --- a/scene/2d/navigation_region_2d.h +++ b/scene/2d/navigation_region_2d.h @@ -99,6 +99,7 @@ class NavigationRegion2D : public Node2D { Ref navpoly; void _navpoly_changed(); + void _map_changed(RID p_RID); protected: void _notification(int p_what); diff --git a/scene/main/viewport.cpp b/scene/main/viewport.cpp index e2b30cfa3d4..e4cda4b96f2 100644 --- a/scene/main/viewport.cpp +++ b/scene/main/viewport.cpp @@ -1163,7 +1163,7 @@ void Viewport::set_world_2d(const Ref &p_world_2d) { if (p_world_2d.is_valid()) { world_2d = p_world_2d; } else { - WARN_PRINT("Invalid world_3d"); + WARN_PRINT("Invalid world_2d"); world_2d = Ref(memnew(World2D)); } diff --git a/scene/resources/world_2d.cpp b/scene/resources/world_2d.cpp index a064ade3627..ccdc5bebd0e 100644 --- a/scene/resources/world_2d.cpp +++ b/scene/resources/world_2d.cpp @@ -368,7 +368,7 @@ World2D::World2D() { navigation_map = NavigationServer2D::get_singleton()->map_create(); NavigationServer2D::get_singleton()->map_set_active(navigation_map, true); NavigationServer2D::get_singleton()->map_set_cell_size(navigation_map, GLOBAL_DEF("navigation/2d/default_cell_size", 10)); - NavigationServer2D::get_singleton()->map_set_edge_connection_margin(navigation_map, GLOBAL_DEF("navigation/2d/default_edge_connection_margin", 100)); + NavigationServer2D::get_singleton()->map_set_edge_connection_margin(navigation_map, GLOBAL_DEF("navigation/2d/default_edge_connection_margin", 5)); indexer = memnew(SpatialIndexer2D); } diff --git a/servers/navigation_server_2d.cpp b/servers/navigation_server_2d.cpp index fee74f3dbc5..7e579c36f49 100644 --- a/servers/navigation_server_2d.cpp +++ b/servers/navigation_server_2d.cpp @@ -156,6 +156,10 @@ static Ref poly_to_mesh(Ref d) { } } +void NavigationServer2D::_emit_map_changed(RID p_map) { + emit_signal("map_changed", p_map); +} + void NavigationServer2D::_bind_methods() { ClassDB::bind_method(D_METHOD("map_create"), &NavigationServer2D::map_create); ClassDB::bind_method(D_METHOD("map_set_active", "map", "active"), &NavigationServer2D::map_set_active); @@ -174,6 +178,9 @@ void NavigationServer2D::_bind_methods() { ClassDB::bind_method(D_METHOD("region_get_layers", "region"), &NavigationServer2D::region_get_layers); ClassDB::bind_method(D_METHOD("region_set_transform", "region", "transform"), &NavigationServer2D::region_set_transform); ClassDB::bind_method(D_METHOD("region_set_navpoly", "region", "nav_poly"), &NavigationServer2D::region_set_navpoly); + ClassDB::bind_method(D_METHOD("region_get_connections_count", "region"), &NavigationServer2D::region_get_connections_count); + ClassDB::bind_method(D_METHOD("region_get_connection_pathway_start", "region", "connection"), &NavigationServer2D::region_get_connection_pathway_start); + ClassDB::bind_method(D_METHOD("region_get_connection_pathway_end", "region", "connection"), &NavigationServer2D::region_get_connection_pathway_end); ClassDB::bind_method(D_METHOD("agent_create"), &NavigationServer2D::agent_create); ClassDB::bind_method(D_METHOD("agent_set_map", "agent", "map"), &NavigationServer2D::agent_set_map); @@ -189,10 +196,14 @@ void NavigationServer2D::_bind_methods() { ClassDB::bind_method(D_METHOD("agent_set_callback", "agent", "receiver", "method", "userdata"), &NavigationServer2D::agent_set_callback, DEFVAL(Variant())); ClassDB::bind_method(D_METHOD("free", "object"), &NavigationServer2D::free); + + ADD_SIGNAL(MethodInfo("map_changed", PropertyInfo(Variant::RID, "map"))); } NavigationServer2D::NavigationServer2D() { singleton = this; + ERR_FAIL_COND_MSG(!NavigationServer3D::get_singleton(), "The Navigation3D singleton should be initialized before the 2D one."); + NavigationServer3D::get_singleton()->connect("map_changed", callable_mp(this, &NavigationServer2D::_emit_map_changed)); } NavigationServer2D::~NavigationServer2D() { @@ -226,6 +237,10 @@ void NavigationServer2D::region_set_navpoly(RID p_region, Ref NavigationServer3D::get_singleton()->region_set_navmesh(p_region, poly_to_mesh(p_nav_mesh)); } +int FORWARD_1_C(region_get_connections_count, RID, p_region, rid_to_rid); +Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_start, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int); +Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_end, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int); + RID NavigationServer2D::agent_create() const { RID agent = NavigationServer3D::get_singleton()->agent_create(); NavigationServer3D::get_singleton()->agent_set_ignore_y(agent, true); diff --git a/servers/navigation_server_2d.h b/servers/navigation_server_2d.h index 46c8f6a71d8..d56c719839f 100644 --- a/servers/navigation_server_2d.h +++ b/servers/navigation_server_2d.h @@ -45,12 +45,14 @@ class NavigationServer2D : public Object { static NavigationServer2D *singleton; + void _emit_map_changed(RID p_map); + protected: static void _bind_methods(); public: /// Thread safe, can be used across many threads. - static const NavigationServer2D *get_singleton() { return singleton; } + static NavigationServer2D *get_singleton() { return singleton; } /// MUST be used in single thread! static NavigationServer2D *get_singleton_mut() { return singleton; } @@ -98,6 +100,11 @@ public: /// Set the navigation poly of this region. virtual void region_set_navpoly(RID p_region, Ref p_nav_mesh) const; + /// Get a list of a region's connection to other regions. + virtual int region_get_connections_count(RID p_region) const; + virtual Vector2 region_get_connection_pathway_start(RID p_region, int p_connection_id) const; + virtual Vector2 region_get_connection_pathway_end(RID p_region, int p_connection_id) const; + /// Creates the agent. virtual RID agent_create() const; diff --git a/servers/navigation_server_3d.cpp b/servers/navigation_server_3d.cpp index d81ce70af61..b0047a250ad 100644 --- a/servers/navigation_server_3d.cpp +++ b/servers/navigation_server_3d.cpp @@ -59,6 +59,9 @@ void NavigationServer3D::_bind_methods() { ClassDB::bind_method(D_METHOD("region_set_transform", "region", "transform"), &NavigationServer3D::region_set_transform); ClassDB::bind_method(D_METHOD("region_set_navmesh", "region", "nav_mesh"), &NavigationServer3D::region_set_navmesh); ClassDB::bind_method(D_METHOD("region_bake_navmesh", "mesh", "node"), &NavigationServer3D::region_bake_navmesh); + ClassDB::bind_method(D_METHOD("region_get_connections_count", "region"), &NavigationServer3D::region_get_connections_count); + ClassDB::bind_method(D_METHOD("region_get_connection_pathway_start", "region", "connection"), &NavigationServer3D::region_get_connection_pathway_start); + ClassDB::bind_method(D_METHOD("region_get_connection_pathway_end", "region", "connection"), &NavigationServer3D::region_get_connection_pathway_end); ClassDB::bind_method(D_METHOD("agent_create"), &NavigationServer3D::agent_create); ClassDB::bind_method(D_METHOD("agent_set_map", "agent", "map"), &NavigationServer3D::agent_set_map); @@ -77,9 +80,11 @@ void NavigationServer3D::_bind_methods() { ClassDB::bind_method(D_METHOD("set_active", "active"), &NavigationServer3D::set_active); ClassDB::bind_method(D_METHOD("process", "delta_time"), &NavigationServer3D::process); + + ADD_SIGNAL(MethodInfo("map_changed", PropertyInfo(Variant::RID, "map"))); } -const NavigationServer3D *NavigationServer3D::get_singleton() { +NavigationServer3D *NavigationServer3D::get_singleton() { return singleton; } diff --git a/servers/navigation_server_3d.h b/servers/navigation_server_3d.h index beed19d563f..420f9c9c182 100644 --- a/servers/navigation_server_3d.h +++ b/servers/navigation_server_3d.h @@ -55,7 +55,7 @@ protected: public: /// Thread safe, can be used across many threads. - static const NavigationServer3D *get_singleton(); + static NavigationServer3D *get_singleton(); /// MUST be used in single thread! static NavigationServer3D *get_singleton_mut(); @@ -111,9 +111,14 @@ public: /// Set the navigation mesh of this region. virtual void region_set_navmesh(RID p_region, Ref p_nav_mesh) const = 0; - /// Bake the navigation mesh + /// Bake the navigation mesh. virtual void region_bake_navmesh(Ref r_mesh, Node *p_node) const = 0; + /// Get a list of a region's connection to other regions. + virtual int region_get_connections_count(RID p_region) const = 0; + virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const = 0; + virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const = 0; + /// Creates the agent. virtual RID agent_create() const = 0;