godotengine
/
godot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Allow Navigation to be more flexible

This commit is contained in:
Gilles Roudière 2021-03-15 12:45:28 +01:00
parent 6eef187a81
commit ac7073f586
18 changed files with 504 additions and 332 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
doc/classes/NavigationServer2D.xml
View File

@ -267,6 +267,37 @@
Creates a new region. Creates a new region.
</description> </description>
</method> </method>
<method name="region_get_connection_pathway_end" qualifiers="const">
<return type="Vector2">
</return>
<argument index="0" name="region" type="RID">
</argument>
<argument index="1" name="connection" type="int">
</argument>
<description>
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].
</description>
</method>
<method name="region_get_connection_pathway_start" qualifiers="const">
<return type="Vector2">
</return>
<argument index="0" name="region" type="RID">
</argument>
<argument index="1" name="connection" type="int">
</argument>
<description>
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].
</description>
</method>
<method name="region_get_connections_count" qualifiers="const">
<return type="int">
</return>
<argument index="0" name="region" type="RID">
</argument>
<description>
Returns how many connections this [code]region[/code] has with other regions in the map.
</description>
</method>
<method name="region_get_layers" qualifiers="const"> <method name="region_get_layers" qualifiers="const">
<return type="int"> <return type="int">
</return> </return>
@ -321,6 +352,15 @@
</description> </description>
</method> </method>
</methods> </methods>
<signals>
<signal name="map_changed">
<argument index="0" name="map" type="RID">
</argument>
<description>
Emitted when a navigation map is updated, when a region moves or is modified.
</description>
</signal>
</signals>
<constants> <constants>
</constants> </constants>
</class> </class>

40
doc/classes/NavigationServer3D.xml
View File

@ -335,6 +335,37 @@
Creates a new region. Creates a new region.
</description> </description>
</method> </method>
<method name="region_get_connection_pathway_end" qualifiers="const">
<return type="Vector3">
</return>
<argument index="0" name="region" type="RID">
</argument>
<argument index="1" name="connection" type="int">
</argument>
<description>
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].
</description>
</method>
<method name="region_get_connection_pathway_start" qualifiers="const">
<return type="Vector3">
</return>
<argument index="0" name="region" type="RID">
</argument>
<argument index="1" name="connection" type="int">
</argument>
<description>
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].
</description>
</method>
<method name="region_get_connections_count" qualifiers="const">
<return type="int">
</return>
<argument index="0" name="region" type="RID">
</argument>
<description>
Returns how many connections this [code]region[/code] has with other regions in the map.
</description>
</method>
<method name="region_get_layers" qualifiers="const"> <method name="region_get_layers" qualifiers="const">
<return type="int"> <return type="int">
</return> </return>
@ -398,6 +429,15 @@
</description> </description>
</method> </method>
</methods> </methods>
<signals>
<signal name="map_changed">
<argument index="0" name="map" type="RID">
</argument>
<description>
Emitted when a navigation map is updated, when a region moves or is modified.
</description>
</signal>
</signals>
<constants> <constants>
</constants> </constants>
</class> </class>

40
modules/gdnavigation/gd_navigation_server.cpp
View File

@ -145,9 +145,13 @@ COMMAND_2(map_set_active, RID, p_map, bool, p_active) {
if (p_active) { if (p_active) {
if (!map_is_active(p_map)) { if (!map_is_active(p_map)) {
active_maps.push_back(map); active_maps.push_back(map);
active_maps_update_id.push_back(map->get_map_update_id());
} }
} else { } 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<NavigationMesh> r_mesh, Node *p
#endif #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 { RID GdNavigationServer::agent_create() const {
auto mut_this = const_cast<GdNavigationServer *>(this); auto mut_this = const_cast<GdNavigationServer *>(this);
MutexLock lock(mut_this->operations_mutex); MutexLock lock(mut_this->operations_mutex);
@ -443,7 +468,9 @@ COMMAND_1(free, RID, p_object) {
agents[i]->set_map(nullptr); 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); map_owner.free(p_object);
memdelete(map); 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 // In c++ we can't be sure that this is performed in the main thread
// even with mutable functions. // even with mutable functions.
MutexLock lock(operations_mutex); 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]->sync();
active_maps[i]->step(p_delta_time); active_maps[i]->step(p_delta_time);
active_maps[i]->dispatch_callbacks(); 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;
}
} }
} }

7
modules/gdnavigation/gd_navigation_server.h
View File

@ -31,6 +31,7 @@
#ifndef GD_NAVIGATION_SERVER_H #ifndef GD_NAVIGATION_SERVER_H
#define GD_NAVIGATION_SERVER_H #define GD_NAVIGATION_SERVER_H
#include "core/templates/local_vector.h"
#include "core/templates/rid.h" #include "core/templates/rid.h"
#include "core/templates/rid_owner.h" #include "core/templates/rid_owner.h"
#include "servers/navigation_server_3d.h" #include "servers/navigation_server_3d.h"
@ -79,7 +80,8 @@ class GdNavigationServer : public NavigationServer3D {
mutable RID_PtrOwner<RvoAgent> agent_owner; mutable RID_PtrOwner<RvoAgent> agent_owner;
bool active = true; bool active = true;
Vector<NavMap *> active_maps; LocalVector<NavMap *> active_maps;
LocalVector<uint32_t> active_maps_update_id;
public: public:
GdNavigationServer(); GdNavigationServer();
@ -114,6 +116,9 @@ public:
COMMAND_2(region_set_transform, RID, p_region, Transform, p_transform); COMMAND_2(region_set_transform, RID, p_region, Transform, p_transform);
COMMAND_2(region_set_navmesh, RID, p_region, Ref<NavigationMesh>, p_nav_mesh); COMMAND_2(region_set_navmesh, RID, p_region, Ref<NavigationMesh>, p_nav_mesh);
virtual void region_bake_navmesh(Ref<NavigationMesh> r_mesh, Node *p_node) const; virtual void region_bake_navmesh(Ref<NavigationMesh> 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; virtual RID agent_create() const;
COMMAND_2(agent_set_map, RID, p_agent, RID, p_map); COMMAND_2(agent_set_map, RID, p_agent, RID, p_map);

422
modules/gdnavigation/nav_map.cpp
View File

@ -40,7 +40,7 @@
@author AndreaCatania @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) { void NavMap::set_up(Vector3 p_up) {
up = p_up; up = p_up;
@ -71,13 +71,13 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
} }
Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers) const { Vector<Vector3> 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 *begin_poly = nullptr;
const gd::Polygon *end_poly = nullptr; const gd::Polygon *end_poly = nullptr;
Vector3 begin_point; Vector3 begin_point;
Vector3 end_point; Vector3 end_point;
float begin_d = 1e20; float begin_d = 1e20;
float end_d = 1e20; float end_d = 1e20;
// Find the initial poly and the end poly on this map. // Find the initial poly and the end poly on this map.
for (size_t i(0); i < polygons.size(); i++) { for (size_t i(0); i < polygons.size(); i++) {
const gd::Polygon &p = polygons[i]; const gd::Polygon &p = polygons[i];
@ -88,31 +88,34 @@ Vector<Vector3> 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 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++) { for (size_t point_id = 0; 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); const Vector3 p1 = p.points[point_id].pos;
Vector3 spoint = f.get_closest_point_to(p_origin); const Vector3 p2 = p.points[(point_id + 1) % p.points.size()].pos;
float dpoint = spoint.distance_to(p_origin); const Vector3 p3 = p.points[(point_id + 2) % p.points.size()].pos;
if (dpoint < begin_d) { const Face3 face(p1, p2, p3);
begin_d = dpoint;
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_poly = &p;
begin_point = spoint; begin_point = point;
} }
spoint = f.get_closest_point_to(p_destination); point = face.get_closest_point_to(p_destination);
dpoint = spoint.distance_to(p_destination); distance_to_point = point.distance_to(p_destination);
if (dpoint < end_d) { if (distance_to_point < end_d) {
end_d = dpoint; end_d = distance_to_point;
end_poly = &p; end_poly = &p;
end_point = spoint; end_point = point;
} }
} }
} }
// Check for trival cases
if (!begin_poly || !end_poly) { if (!begin_poly || !end_poly) {
// No path
return Vector<Vector3>(); return Vector<Vector3>();
} }
if (begin_poly == end_poly) { if (begin_poly == end_poly) {
Vector<Vector3> path; Vector<Vector3> path;
path.resize(2); path.resize(2);
@ -121,95 +124,89 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
return path; return path;
} }
// List of all reachable navigation polys.
std::vector<gd::NavigationPoly> navigation_polys; std::vector<gd::NavigationPoly> navigation_polys;
navigation_polys.reserve(polygons.size() * 0.75); navigation_polys.reserve(polygons.size() * 0.75);
// The elements indices in the `navigation_polys`. // Add the start polygon to the reachable navigation polygons.
int least_cost_id(-1); gd::NavigationPoly begin_navigation_poly = gd::NavigationPoly(begin_poly);
List<uint32_t> open_list; 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<uint32_t> to_visit;
to_visit.push_back(0);
// This is an implementation of the A* algorithm.
int least_cost_id = 0;
bool found_route = false; 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; const gd::Polygon *reachable_end = nullptr;
float reachable_d = 1e30; float reachable_d = 1e30;
bool is_reachable = true; bool is_reachable = true;
while (found_route == false) { while (true) {
{
// 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]; gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
// Only consider the polygon if it in a region with compatible layers. // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
if ((p_layers & least_cost_poly->poly->owner->get_layers()) == 0) { for (size_t i = 0; i < least_cost_poly->poly->edges.size(); i++) {
continue;
}
const gd::Edge &edge = least_cost_poly->poly->edges[i]; const gd::Edge &edge = least_cost_poly->poly->edges[i];
if (!edge.other_polygon) {
// 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; continue;
} }
#ifdef USE_ENTRY_POINT Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end };
Vector3 edge_line[2] = { const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly->entry, pathway);
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);
const float new_distance = least_cost_poly->entry.distance_to(new_entry) + least_cost_poly->traveled_distance; 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( auto it = std::find(
navigation_polys.begin(), navigation_polys.begin(),
navigation_polys.end(), navigation_polys.end(),
gd::NavigationPoly(edge.other_polygon)); gd::NavigationPoly(connection.polygon));
if (it != navigation_polys.end()) { if (it != navigation_polys.end()) {
// Oh this was visited already, can we win the cost? // Polygon already visited, check if we can reduce the travel cost.
if (it->traveled_distance > new_distance) { if (new_distance < it->traveled_distance) {
it->prev_navigation_poly_id = least_cost_id; it->back_navigation_poly_id = least_cost_id;
it->back_navigation_edge = edge.other_edge; 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; it->traveled_distance = new_distance;
#ifdef USE_ENTRY_POINT
it->entry = new_entry; it->entry = new_entry;
#endif
} }
} else { } 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)); // Add the neighbour polygon to the polygons to visit.
gd::NavigationPoly *np = &navigation_polys[navigation_polys.size() - 1]; to_visit.push_back(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);
} }
} }
} }
// Removes the least cost polygon from the open list so we can advance. // Removes the least cost polygon from the list of polygons to visit so we can advance.
open_list.erase(least_cost_id); to_visit.erase(least_cost_id);
if (open_list.size() == 0) { // When the list of polygons to visit is empty at this point it means the End Polygon is not reachable
// When the open list is empty at this point the End Polygon is not reachable if (to_visit.size() == 0) {
// so use the further reachable polygon // Thus use the further reachable polygon
ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons"); ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons");
is_reachable = false; is_reachable = false;
if (reachable_end == nullptr) { if (reachable_end == nullptr) {
@ -234,26 +231,21 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
gd::NavigationPoly np = navigation_polys[0]; gd::NavigationPoly np = navigation_polys[0];
navigation_polys.clear(); navigation_polys.clear();
navigation_polys.push_back(np); navigation_polys.push_back(np);
open_list.clear(); to_visit.clear();
open_list.push_back(0); to_visit.push_back(0);
reachable_end = nullptr; reachable_end = nullptr;
continue; 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; least_cost_id = -1;
float least_cost = 1e30; float least_cost = 1e30;
for (List<uint32_t>::Element *element = to_visit.front(); element != nullptr; element = element->next()) {
for (auto element = open_list.front(); element != nullptr; element = element->next()) {
gd::NavigationPoly *np = &navigation_polys[element->get()]; gd::NavigationPoly *np = &navigation_polys[element->get()];
float cost = np->traveled_distance; float cost = np->traveled_distance;
#ifdef USE_ENTRY_POINT
cost += np->entry.distance_to(end_point); cost += np->entry.distance_to(end_point);
#else
cost += np->poly->center.distance_to(end_point);
#endif
if (cost < least_cost) { if (cost < least_cost) {
least_cost_id = np->self_id; least_cost_id = np->self_id;
least_cost = cost; least_cost = cost;
@ -273,94 +265,88 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
// Check if we reached the end // Check if we reached the end
if (navigation_polys[least_cost_id].poly == end_poly) { if (navigation_polys[least_cost_id].poly == end_poly) {
// Yep, done!!
found_route = true; found_route = true;
break; break;
} }
} }
if (found_route) { // If we did not find a route, return an empty path.
Vector<Vector3> path; if (!found_route) {
if (p_optimize) { return Vector<Vector3>();
// String pulling }
Vector<Vector3> 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]; gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
Vector3 apex_point = end_point; Vector3 apex_point = end_point;
Vector3 portal_left = apex_point;
Vector3 portal_right = apex_point;
gd::NavigationPoly *left_poly = apex_poly; gd::NavigationPoly *left_poly = apex_poly;
Vector3 left_portal = apex_point;
gd::NavigationPoly *right_poly = apex_poly; gd::NavigationPoly *right_poly = apex_poly;
Vector3 right_portal = apex_point;
gd::NavigationPoly *p = apex_poly; gd::NavigationPoly *p = apex_poly;
path.push_back(end_point); path.push_back(end_point);
while (p) { while (p) {
Vector3 left; // Set left and right points of the pathway between polygons.
Vector3 right; Vector3 left = p->back_navigation_edge_pathway_start;
Vector3 right = p->back_navigation_edge_pathway_end;
#define CLOCK_TANGENT(m_a, m_b, m_c) (((m_a) - (m_c)).cross((m_a) - (m_b))) if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(up) < 0) {
if (p->poly == begin_poly) {
left = begin_point;
right = begin_point;
} 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;
if (p->poly->clockwise) {
SWAP(left, right); SWAP(left, right);
} }
}
bool skip = false; bool skip = false;
if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(up) >= 0) {
if (CLOCK_TANGENT(apex_point, portal_left, left).dot(up) >= 0) {
//process //process
if (portal_left == apex_point || CLOCK_TANGENT(apex_point, left, portal_right).dot(up) > 0) { if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(up) > 0) {
left_poly = p; left_poly = p;
portal_left = left; left_portal = left;
} else { } else {
clip_path(navigation_polys, path, apex_poly, portal_right, right_poly); clip_path(navigation_polys, path, apex_poly, right_portal, right_poly);
apex_point = portal_right; apex_point = right_portal;
p = right_poly; p = right_poly;
left_poly = p; left_poly = p;
apex_poly = p; apex_poly = p;
portal_left = apex_point; left_portal = apex_point;
portal_right = apex_point; right_portal = apex_point;
path.push_back(apex_point); path.push_back(apex_point);
skip = true; skip = true;
} }
} }
if (!skip && CLOCK_TANGENT(apex_point, portal_right, right).dot(up) <= 0) { if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(up) <= 0) {
//process //process
if (portal_right == apex_point || CLOCK_TANGENT(apex_point, right, portal_left).dot(up) < 0) { if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(up) < 0) {
right_poly = p; right_poly = p;
portal_right = right; right_portal = right;
} else { } else {
clip_path(navigation_polys, path, apex_poly, portal_left, left_poly); clip_path(navigation_polys, path, apex_poly, left_portal, left_poly);
apex_point = portal_left; apex_point = left_portal;
p = left_poly; p = left_poly;
right_poly = p; right_poly = p;
apex_poly = p; apex_poly = p;
portal_right = apex_point; right_portal = apex_point;
portal_left = apex_point; left_portal = apex_point;
path.push_back(apex_point); path.push_back(apex_point);
} }
} }
if (p->prev_navigation_poly_id != -1) { // Go to the previous polygon.
p = &navigation_polys[p->prev_navigation_poly_id]; if (p->back_navigation_poly_id != -1) {
p = &navigation_polys[p->back_navigation_poly_id];
} else { } else {
// The end // The end
p = nullptr; p = nullptr;
} }
} }
// If the last point is not the begin point, add it to the list.
if (path[path.size() - 1] != begin_point) { if (path[path.size() - 1] != begin_point) {
path.push_back(begin_point); path.push_back(begin_point);
} }
@ -373,24 +359,14 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
// Add mid points // Add mid points
int np_id = least_cost_id; int np_id = least_cost_id;
while (np_id != -1) { while (np_id != -1) {
#ifdef USE_ENTRY_POINT path.push_back(navigation_polys[np_id].entry);
Vector3 point = navigation_polys[np_id].entry; np_id = navigation_polys[np_id].back_navigation_poly_id;
#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(); path.invert();
} }
return path; return path;
}
return Vector<Vector3>();
} }
Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { 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() { void NavMap::sync() {
// Check if we need to update the links.
if (regenerate_polygons) { if (regenerate_polygons) {
for (size_t r(0); r < regions.size(); r++) { for (size_t r(0); r < regions.size(); r++) {
regions[r]->scratch_polygons(); regions[r]->scratch_polygons();
@ -585,27 +562,30 @@ void NavMap::sync() {
} }
if (regenerate_links) { 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; int count = 0;
for (size_t r(0); r < regions.size(); r++) { for (size_t r(0); r < regions.size(); r++) {
count += regions[r]->get_polygons().size(); count += regions[r]->get_polygons().size();
} }
polygons.resize(count); polygons.resize(count);
count = 0;
// Copy all region polygons in the map.
count = 0;
for (size_t r(0); r < regions.size(); r++) { for (size_t r(0); r < regions.size(); r++) {
std::copy( std::copy(
regions[r]->get_polygons().data(), regions[r]->get_polygons().data(),
regions[r]->get_polygons().data() + regions[r]->get_polygons().size(), regions[r]->get_polygons().data() + regions[r]->get_polygons().size(),
polygons.begin() + count); polygons.begin() + count);
count += regions[r]->get_polygons().size(); count += regions[r]->get_polygons().size();
} }
// Connects the `Edges` of all the `Polygons` of all `Regions` each other. // Group all edges per key.
Map<gd::EdgeKey, gd::Connection> connections; Map<gd::EdgeKey, Vector<gd::Edge::Connection>> connections;
for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) { for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) {
gd::Polygon &poly(polygons[poly_id]); gd::Polygon &poly(polygons[poly_id]);
@ -613,30 +593,18 @@ void NavMap::sync() {
int next_point = (p + 1) % poly.points.size(); int next_point = (p + 1) % poly.points.size();
gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key); gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
Map<gd::EdgeKey, gd::Connection>::Element *connection = connections.find(ek); Map<gd::EdgeKey, Vector<gd::Edge::Connection>>::Element *connection = connections.find(ek);
if (!connection) { if (!connection) {
// Nothing yet connections[ek] = Vector<gd::Edge::Connection>();
gd::Connection c; }
c.A = &poly; if (connections[ek].size() <= 1) {
c.A_edge = p; // Add the polygon/edge tuple to this key.
c.B = nullptr; gd::Edge::Connection new_connection;
c.B_edge = -1; new_connection.polygon = &poly;
connections[ek] = c; new_connection.edge = p;
new_connection.pathway_start = poly.points[p].pos;
} else if (connection->get().B == nullptr) { new_connection.pathway_end = poly.points[next_point].pos;
CRASH_COND(connection->get().A == nullptr); // Unreachable connections[ek].push_back(new_connection);
// 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;
} else { } else {
// The edge is already connected with another edge, skip. // 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."); 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. Vector<gd::Edge::Connection> free_edges;
std::vector<gd::FreeEdge> free_edges; for (Map<gd::EdgeKey, Vector<gd::Edge::Connection>>::Element *E = connections.front(); E; E = E->next()) {
free_edges.reserve(connections.size()); if (E->get().size() == 2) {
// Connect edge that are shared in different polygons.
for (auto connection_element = connections.front(); connection_element; connection_element = connection_element->next()) { gd::Edge::Connection &c1 = E->get().write[0];
if (connection_element->get().B == nullptr) { gd::Edge::Connection &c2 = E->get().write[1];
CRASH_COND(connection_element->get().A == nullptr); // Unreachable c1.polygon->edges[c1.edge].connections.push_back(c2);
CRASH_COND(connection_element->get().A_edge < 0); // Unreachable 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.
// This is a free edge } else {
uint32_t id(free_edges.size()); CRASH_COND_MSG(E->get().size() != 1, vformat("Number of connection != 1. Found: %d", E->get().size()));
free_edges.push_back(gd::FreeEdge()); free_edges.push_back(E->get()[0]);
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();
} }
} }
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. // Find the compatible near edges.
// //
// Note: // Note:
@ -683,43 +634,67 @@ void NavMap::sync() {
// to be connected, create new polygons to remove that small gap is // to be connected, create new polygons to remove that small gap is
// not really useful and would result in wasteful computation during // not really useful and would result in wasteful computation during
// connection, integration and path finding. // connection, integration and path finding.
for (size_t i(0); i < free_edges.size(); i++) { for (int i = 0; i < free_edges.size(); i++) {
if (!free_edges[i].is_free) { const gd::Edge::Connection &free_edge = free_edges[i];
continue; 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;
gd::FreeEdge &edge = free_edges[i];
for (size_t y(0); y < free_edges.size(); y++) { for (int j = 0; j < free_edges.size(); j++) {
gd::FreeEdge &other_edge = free_edges[y]; const gd::Edge::Connection &other_edge = free_edges[j];
if (i == y || !other_edge.is_free || edge.poly->owner == other_edge.poly->owner) { if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
continue; continue;
} }
Vector3 rel_centers = other_edge.edge_center - edge.edge_center; Vector3 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos;
if (ecm_squared > rel_centers.length_squared() // Are enough closer? Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
&& 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;
edge.poly->edges[edge.edge_id].this_edge = edge.edge_id; // Compute the projection of the opposite edge on the current one
edge.poly->edges[edge.edge_id].other_edge = other_edge.edge_id; Vector3 edge_vector = edge_p2 - edge_p1;
edge.poly->edges[edge.edge_id].other_polygon = other_edge.poly; 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;
}
other_edge.poly->edges[other_edge.edge_id].this_edge = other_edge.edge_id; // Check if the two edges are close to each other enough and compute a pathway between the two regions.
other_edge.poly->edges[other_edge.edge_id].other_edge = edge.edge_id; Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
other_edge.poly->edges[other_edge.edge_id].other_polygon = edge.poly; 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; map_update_id = (map_update_id + 1) % 9999999;
} }
// Update agents tree.
if (agents_dirty) { if (agents_dirty) {
std::vector<RVO::Agent *> raw_agents; std::vector<RVO::Agent *> raw_agents;
raw_agents.reserve(agents.size()); raw_agents.reserve(agents.size());
@ -771,16 +746,15 @@ void NavMap::clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys
cut_plane.d = cut_plane.normal.dot(from); cut_plane.d = cut_plane.normal.dot(from);
while (from_poly != p_to_poly) { while (from_poly != p_to_poly) {
int back_nav_edge = from_poly->back_navigation_edge; Vector3 pathway_start = from_poly->back_navigation_edge_pathway_start;
Vector3 a = from_poly->poly->points[back_nav_edge].pos; Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end;
Vector3 b = from_poly->poly->points[(back_nav_edge + 1) % from_poly->poly->points.size()].pos;
ERR_FAIL_COND(from_poly->prev_navigation_poly_id == -1); ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1);
from_poly = &p_navigation_polys[from_poly->prev_navigation_poly_id]; 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; 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) { if (inters.distance_to(p_to_point) > CMP_EPSILON && inters.distance_to(path[path.size() - 1]) > CMP_EPSILON) {
path.push_back(inters); path.push_back(inters);
} }

1
modules/gdnavigation/nav_map.h
View File

@ -34,6 +34,7 @@
#include "nav_rid.h" #include "nav_rid.h"
#include "core/math/math_defs.h" #include "core/math/math_defs.h"
#include "core/templates/map.h"
#include "nav_utils.h" #include "nav_utils.h"
#include <KdTree.h> #include <KdTree.h>

22
modules/gdnavigation/nav_region.cpp
View File

@ -39,6 +39,9 @@
void NavRegion::set_map(NavMap *p_map) { void NavRegion::set_map(NavMap *p_map) {
map = p_map; map = p_map;
polygons_dirty = true; polygons_dirty = true;
if (!map) {
connections.clear();
}
} }
void NavRegion::set_layers(uint32_t p_layers) { void NavRegion::set_layers(uint32_t p_layers) {
@ -59,6 +62,25 @@ void NavRegion::set_mesh(Ref<NavigationMesh> p_mesh) {
polygons_dirty = true; 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 NavRegion::sync() {
bool something_changed = polygons_dirty /* || something_dirty? */; bool something_changed = polygons_dirty /* || something_dirty? */;

8
modules/gdnavigation/nav_region.h
View File

@ -49,6 +49,7 @@ class NavRegion : public NavRid {
Transform transform; Transform transform;
Ref<NavigationMesh> mesh; Ref<NavigationMesh> mesh;
uint32_t layers = 1; uint32_t layers = 1;
Vector<gd::Edge::Connection> connections;
bool polygons_dirty = true; bool polygons_dirty = true;
@ -80,6 +81,13 @@ public:
return mesh; return mesh;
} }
Vector<gd::Edge::Connection> &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<gd::Polygon> const &get_polygons() const { std::vector<gd::Polygon> const &get_polygons() const {
return polygons; return polygons;
} }

39
modules/gdnavigation/nav_utils.h
View File

@ -81,11 +81,14 @@ struct Edge {
/// This edge ID /// This edge ID
int this_edge = -1; int this_edge = -1;
/// Other Polygon /// The gateway in the edge, as, in some case, the whole edge might not be navigable.
Polygon *other_polygon = nullptr; struct Connection {
Polygon *polygon = nullptr;
/// The other `Polygon` at this edge id has this `Polygon`. int edge = -1;
int other_edge = -1; Vector3 pathway_start;
Vector3 pathway_end;
};
Vector<Connection> connections;
}; };
struct Polygon { struct Polygon {
@ -104,21 +107,17 @@ struct Polygon {
Vector3 center; Vector3 center;
}; };
struct Connection {
Polygon *A = nullptr;
int A_edge = -1;
Polygon *B = nullptr;
int B_edge = -1;
};
struct NavigationPoly { struct NavigationPoly {
uint32_t self_id = 0; uint32_t self_id = 0;
/// This poly. /// This poly.
const Polygon *poly; const Polygon *poly;
/// The previous navigation poly (id in the `navigation_poly` array).
int prev_navigation_poly_id = -1; /// Those 4 variables are used to travel the path backwards.
/// The edge id in this `Poly` to reach the `prev_navigation_poly_id`. int back_navigation_poly_id = -1;
uint32_t back_navigation_edge = 0; 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. /// The entry location of this poly.
Vector3 entry; Vector3 entry;
/// The distance to the destination. /// 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 } // namespace gd
#endif // NAV_UTILS_H #endif // NAV_UTILS_H

6
modules/gridmap/doc_classes/GridMap.xml
View File

@ -214,12 +214,12 @@
<member name="collision_mask" type="int" setter="set_collision_mask" getter="get_collision_mask" default="1"> <member name="collision_mask" type="int" setter="set_collision_mask" getter="get_collision_mask" default="1">
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 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.
</member> </member>
<member name="navigation_layers" type="int" setter="set_navigation_layers" getter="get_navigation_layers" default="1">
The navigation layers the GridMap generates its navigable regions in.
</member>
<member name="mesh_library" type="MeshLibrary" setter="set_mesh_library" getter="get_mesh_library"> <member name="mesh_library" type="MeshLibrary" setter="set_mesh_library" getter="get_mesh_library">
The assigned [MeshLibrary]. The assigned [MeshLibrary].
</member> </member>
<member name="navigation_layers" type="int" setter="set_navigation_layers" getter="get_navigation_layers" default="1">
The navigation layers the GridMap generates its navigable regions in.
</member>
</members> </members>
<signals> <signals>
<signal name="cell_size_changed"> <signal name="cell_size_changed">

68
scene/2d/navigation_region_2d.cpp
View File

@ -364,8 +364,10 @@ void NavigationRegion2D::set_enabled(bool p_enabled) {
if (!enabled) { if (!enabled) {
NavigationServer2D::get_singleton()->region_set_map(region, RID()); NavigationServer2D::get_singleton()->region_set_map(region, RID());
NavigationServer2D::get_singleton()->disconnect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed));
} else { } else {
NavigationServer2D::get_singleton()->region_set_map(region, get_world_2d()->get_navigation_map()); 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()) { 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: { case NOTIFICATION_ENTER_TREE: {
if (enabled) { if (enabled) {
NavigationServer2D::get_singleton()->region_set_map(region, get_world_2d()->get_navigation_map()); 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; } break;
case NOTIFICATION_TRANSFORM_CHANGED: { case NOTIFICATION_TRANSFORM_CHANGED: {
@ -408,12 +411,14 @@ void NavigationRegion2D::_notification(int p_what) {
} break; } break;
case NOTIFICATION_EXIT_TREE: { case NOTIFICATION_EXIT_TREE: {
NavigationServer2D::get_singleton()->region_set_map(region, RID()); NavigationServer2D::get_singleton()->region_set_map(region, RID());
if (enabled) {
NavigationServer2D::get_singleton()->disconnect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed));
}
} break; } break;
case NOTIFICATION_DRAW: { case NOTIFICATION_DRAW: {
if (is_inside_tree() && (Engine::get_singleton()->is_editor_hint() || get_tree()->is_debugging_navigation_hint()) && navpoly.is_valid()) { if (is_inside_tree() && (Engine::get_singleton()->is_editor_hint() || get_tree()->is_debugging_navigation_hint()) && navpoly.is_valid()) {
Vector<Vector2> verts = navpoly->get_vertices(); Vector<Vector2> verts = navpoly->get_vertices();
int vsize = verts.size(); if (verts.size() < 3) {
if (vsize < 3) {
return; return;
} }
@ -423,34 +428,48 @@ void NavigationRegion2D::_notification(int p_what) {
} else { } else {
color = get_tree()->get_debug_navigation_disabled_color(); color = get_tree()->get_debug_navigation_disabled_color();
} }
Vector<Color> colors; Color doors_color = color.lightened(0.2);
Vector<Vector2> 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;
}
}
Vector<int> indices; RandomPCG rand;
for (int i = 0; i < navpoly->get_polygon_count(); i++) { for (int i = 0; i < navpoly->get_polygon_count(); i++) {
// An array of vertices for this polygon.
Vector<int> polygon = navpoly->get_polygon(i); Vector<int> polygon = navpoly->get_polygon(i);
Vector<Vector2> 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]];
}
for (int j = 2; j < polygon.size(); j++) { // Generate the polygon color, slightly randomly modified from the settings one.
int kofs[3] = { 0, j - 1, j }; Color random_variation_color;
for (int k = 0; k < 3; k++) { 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);
int idx = polygon[kofs[k]]; random_variation_color.a = color.a;
ERR_FAIL_INDEX(idx, vsize); Vector<Color> colors;
indices.push_back(idx); 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; } break;
} }
} }
@ -484,6 +503,11 @@ void NavigationRegion2D::_navpoly_changed() {
update(); 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 { String NavigationRegion2D::get_configuration_warning() const {
if (!is_visible_in_tree() || !is_inside_tree()) { if (!is_visible_in_tree() || !is_inside_tree()) {

1
scene/2d/navigation_region_2d.h
View File

@ -99,6 +99,7 @@ class NavigationRegion2D : public Node2D {
Ref<NavigationPolygon> navpoly; Ref<NavigationPolygon> navpoly;
void _navpoly_changed(); void _navpoly_changed();
void _map_changed(RID p_RID);
protected: protected:
void _notification(int p_what); void _notification(int p_what);

2
scene/main/viewport.cpp
View File

@ -1163,7 +1163,7 @@ void Viewport::set_world_2d(const Ref<World2D> &p_world_2d) {
if (p_world_2d.is_valid()) { if (p_world_2d.is_valid()) {
world_2d = p_world_2d; world_2d = p_world_2d;
} else { } else {
WARN_PRINT("Invalid world_3d"); WARN_PRINT("Invalid world_2d");
world_2d = Ref<World2D>(memnew(World2D)); world_2d = Ref<World2D>(memnew(World2D));
} }

2
scene/resources/world_2d.cpp
View File

@ -368,7 +368,7 @@ World2D::World2D() {
navigation_map = NavigationServer2D::get_singleton()->map_create(); navigation_map = NavigationServer2D::get_singleton()->map_create();
NavigationServer2D::get_singleton()->map_set_active(navigation_map, true); 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_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); indexer = memnew(SpatialIndexer2D);
} }

15
servers/navigation_server_2d.cpp
View File

@ -156,6 +156,10 @@ static Ref<NavigationMesh> poly_to_mesh(Ref<NavigationPolygon> d) {
} }
} }
void NavigationServer2D::_emit_map_changed(RID p_map) {
emit_signal("map_changed", p_map);
}
void NavigationServer2D::_bind_methods() { void NavigationServer2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("map_create"), &NavigationServer2D::map_create); ClassDB::bind_method(D_METHOD("map_create"), &NavigationServer2D::map_create);
ClassDB::bind_method(D_METHOD("map_set_active", "map", "active"), &NavigationServer2D::map_set_active); 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_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_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_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_create"), &NavigationServer2D::agent_create);
ClassDB::bind_method(D_METHOD("agent_set_map", "agent", "map"), &NavigationServer2D::agent_set_map); 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("agent_set_callback", "agent", "receiver", "method", "userdata"), &NavigationServer2D::agent_set_callback, DEFVAL(Variant()));
ClassDB::bind_method(D_METHOD("free", "object"), &NavigationServer2D::free); ClassDB::bind_method(D_METHOD("free", "object"), &NavigationServer2D::free);
ADD_SIGNAL(MethodInfo("map_changed", PropertyInfo(Variant::RID, "map")));
} }
NavigationServer2D::NavigationServer2D() { NavigationServer2D::NavigationServer2D() {
singleton = this; 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() { NavigationServer2D::~NavigationServer2D() {
@ -226,6 +237,10 @@ void NavigationServer2D::region_set_navpoly(RID p_region, Ref<NavigationPolygon>
NavigationServer3D::get_singleton()->region_set_navmesh(p_region, poly_to_mesh(p_nav_mesh)); 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 NavigationServer2D::agent_create() const {
RID agent = NavigationServer3D::get_singleton()->agent_create(); RID agent = NavigationServer3D::get_singleton()->agent_create();
NavigationServer3D::get_singleton()->agent_set_ignore_y(agent, true); NavigationServer3D::get_singleton()->agent_set_ignore_y(agent, true);

9
servers/navigation_server_2d.h
View File

@ -45,12 +45,14 @@ class NavigationServer2D : public Object {
static NavigationServer2D *singleton; static NavigationServer2D *singleton;
void _emit_map_changed(RID p_map);
protected: protected:
static void _bind_methods(); static void _bind_methods();
public: public:
/// Thread safe, can be used across many threads. /// 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! /// MUST be used in single thread!
static NavigationServer2D *get_singleton_mut() { return singleton; } static NavigationServer2D *get_singleton_mut() { return singleton; }
@ -98,6 +100,11 @@ public:
/// Set the navigation poly of this region. /// Set the navigation poly of this region.
virtual void region_set_navpoly(RID p_region, Ref<NavigationPolygon> p_nav_mesh) const; virtual void region_set_navpoly(RID p_region, Ref<NavigationPolygon> 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. /// Creates the agent.
virtual RID agent_create() const; virtual RID agent_create() const;

7
servers/navigation_server_3d.cpp
View File

@ -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_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_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_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_create"), &NavigationServer3D::agent_create);
ClassDB::bind_method(D_METHOD("agent_set_map", "agent", "map"), &NavigationServer3D::agent_set_map); 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("set_active", "active"), &NavigationServer3D::set_active);
ClassDB::bind_method(D_METHOD("process", "delta_time"), &NavigationServer3D::process); 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; return singleton;
} }

9
servers/navigation_server_3d.h
View File

@ -55,7 +55,7 @@ protected:
public: public:
/// Thread safe, can be used across many threads. /// Thread safe, can be used across many threads.
static const NavigationServer3D *get_singleton(); static NavigationServer3D *get_singleton();
/// MUST be used in single thread! /// MUST be used in single thread!
static NavigationServer3D *get_singleton_mut(); static NavigationServer3D *get_singleton_mut();
@ -111,9 +111,14 @@ public:
/// Set the navigation mesh of this region. /// Set the navigation mesh of this region.
virtual void region_set_navmesh(RID p_region, Ref<NavigationMesh> p_nav_mesh) const = 0; virtual void region_set_navmesh(RID p_region, Ref<NavigationMesh> p_nav_mesh) const = 0;
/// Bake the navigation mesh /// Bake the navigation mesh.
virtual void region_bake_navmesh(Ref<NavigationMesh> r_mesh, Node *p_node) const = 0; virtual void region_bake_navmesh(Ref<NavigationMesh> 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. /// Creates the agent.
virtual RID agent_create() const = 0; virtual RID agent_create() const = 0;