mirror/godot
2
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-04-07 00:44:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #102100 from AThousandShips/nav_split_prepare

Browse Source 
[Navigation] Rename classes in preparation for future restructure
This commit is contained in:
Thaddeus Crews 2025-03-12 13:17:11 -05:00
commit c6004c6267
No known key found for this signature in database
GPG Key ID: 62181B86FE9E5D84
28 changed files with 1075 additions and 999 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
misc/error_suppressions/tsan.txt
View File

@ -4,4 +4,4 @@
deadlock:tests/core/templates/test_command_queue.h
deadlock:modules/text_server_adv/text_server_adv.cpp
deadlock:modules/text_server_fb/text_server_fb.cpp
race:modules/navigation/nav_map.cpp
race:modules/navigation/nav_map_3d.cpp

10
modules/navigation/2d/godot_navigation_server_2d.h
View File

@ -30,11 +30,11 @@
#pragma once
#include "../nav_agent.h"
#include "../nav_link.h"
#include "../nav_map.h"
#include "../nav_obstacle.h"
#include "../nav_region.h"
#include "../nav_agent_3d.h"
#include "../nav_link_3d.h"
#include "../nav_map_3d.h"
#include "../nav_obstacle_3d.h"
#include "../nav_region_3d.h"
#include "servers/navigation_server_2d.h"

332
modules/navigation/3d/godot_navigation_server_3d.cpp
View File

File diff suppressed because it is too large Load Diff

30
modules/navigation/3d/godot_navigation_server_3d.h
View File

@ -30,11 +30,11 @@
#pragma once
#include "../nav_agent.h"
#include "../nav_link.h"
#include "../nav_map.h"
#include "../nav_obstacle.h"
#include "../nav_region.h"
#include "../nav_agent_3d.h"
#include "../nav_link_3d.h"
#include "../nav_map_3d.h"
#include "../nav_obstacle_3d.h"
#include "../nav_region_3d.h"
#include "core/templates/local_vector.h"
#include "core/templates/rid.h"
@ -61,8 +61,8 @@ class GodotNavigationServer3D;
class NavMeshGenerator3D;
#endif // _3D_DISABLED
struct SetCommand {
virtual ~SetCommand() {}
struct SetCommand3D {
virtual ~SetCommand3D() {}
virtual void exec(GodotNavigationServer3D *server) = 0;
};
@ -71,16 +71,16 @@ class GodotNavigationServer3D : public NavigationServer3D {
/// Mutex used to make any operation threadsafe.
Mutex operations_mutex;
LocalVector<SetCommand *> commands;
LocalVector<SetCommand3D *> commands;
mutable RID_Owner<NavLink> link_owner;
mutable RID_Owner<NavMap> map_owner;
mutable RID_Owner<NavRegion> region_owner;
mutable RID_Owner<NavAgent> agent_owner;
mutable RID_Owner<NavObstacle> obstacle_owner;
mutable RID_Owner<NavLink3D> link_owner;
mutable RID_Owner<NavMap3D> map_owner;
mutable RID_Owner<NavRegion3D> region_owner;
mutable RID_Owner<NavAgent3D> agent_owner;
mutable RID_Owner<NavObstacle3D> obstacle_owner;
bool active = true;
LocalVector<NavMap *> active_maps;
LocalVector<NavMap3D *> active_maps;
LocalVector<uint32_t> active_maps_iteration_id;
#ifndef _3D_DISABLED
@ -102,7 +102,7 @@ public:
GodotNavigationServer3D();
virtual ~GodotNavigationServer3D();
void add_command(SetCommand *command);
void add_command(SetCommand3D *command);
virtual TypedArray<RID> get_maps() const override;

8
modules/navigation/3d/nav_base_iteration_3d.h
View File

@ -30,11 +30,11 @@
#pragma once
#include "../nav_utils.h"
#include "../nav_utils_3d.h"
#include "servers/navigation/navigation_utilities.h"
struct NavBaseIteration {
struct NavBaseIteration3D {
uint32_t id = UINT32_MAX;
bool enabled = true;
uint32_t navigation_layers = 1;
@ -44,7 +44,7 @@ struct NavBaseIteration {
ObjectID owner_object_id;
RID owner_rid;
bool owner_use_edge_connections = false;
LocalVector<gd::Polygon> navmesh_polygons;
LocalVector<nav_3d::Polygon> navmesh_polygons;
bool get_enabled() const { return enabled; }
NavigationUtilities::PathSegmentType get_type() const { return owner_type; }
@ -54,5 +54,5 @@ struct NavBaseIteration {
real_t get_enter_cost() const { return enter_cost; }
real_t get_travel_cost() const { return travel_cost; }
bool get_use_edge_connections() const { return owner_use_edge_connections; }
const LocalVector<gd::Polygon> &get_navmesh_polygons() const { return navmesh_polygons; }
const LocalVector<nav_3d::Polygon> &get_navmesh_polygons() const { return navmesh_polygons; }
};

123
modules/navigation/3d/nav_map_builder_3d.cpp
View File

@ -32,18 +32,20 @@
#include "nav_map_builder_3d.h"
#include "../nav_link.h"
#include "../nav_map.h"
#include "../nav_region.h"
#include "../nav_link_3d.h"
#include "../nav_map_3d.h"
#include "../nav_region_3d.h"
#include "nav_map_iteration_3d.h"
#include "nav_region_iteration_3d.h"
gd::PointKey NavMapBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size) {
using namespace nav_3d;
PointKey NavMapBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size) {
const int x = static_cast<int>(Math::floor(p_pos.x / p_cell_size.x));
const int y = static_cast<int>(Math::floor(p_pos.y / p_cell_size.y));
const int z = static_cast<int>(Math::floor(p_pos.z / p_cell_size.z));
gd::PointKey p;
PointKey p;
p.key = 0;
p.x = x;
p.y = y;
@ -51,8 +53,8 @@ gd::PointKey NavMapBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3
return p;
}
void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild &r_build) {
gd::PerformanceData &performance_data = r_build.performance_data;
void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild3D &r_build) {
PerformanceData &performance_data = r_build.performance_data;
performance_data.pm_polygon_count = 0;
performance_data.pm_edge_count = 0;
@ -73,26 +75,26 @@ void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild &r_build) {
_build_update_map_iteration(r_build);
}
void NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild &r_build) {
gd::PerformanceData &performance_data = r_build.performance_data;
NavMapIteration *map_iteration = r_build.map_iteration;
void NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild3D &r_build) {
PerformanceData &performance_data = r_build.performance_data;
NavMapIteration3D *map_iteration = r_build.map_iteration;
LocalVector<NavRegionIteration> &regions = map_iteration->region_iterations;
HashMap<uint32_t, LocalVector<gd::Edge::Connection>> &region_external_connections = map_iteration->external_region_connections;
LocalVector<NavRegionIteration3D> &regions = map_iteration->region_iterations;
HashMap<uint32_t, LocalVector<Edge::Connection>> &region_external_connections = map_iteration->external_region_connections;
// Remove regions connections.
region_external_connections.clear();
for (const NavRegionIteration &region : regions) {
region_external_connections[region.id] = LocalVector<gd::Edge::Connection>();
for (const NavRegionIteration3D &region : regions) {
region_external_connections[region.id] = LocalVector<Edge::Connection>();
}
// Copy all region polygons in the map.
int polygon_count = 0;
for (NavRegionIteration &region : regions) {
for (NavRegionIteration3D &region : regions) {
if (!region.get_enabled()) {
continue;
}
LocalVector<gd::Polygon> &polygons_source = region.navmesh_polygons;
LocalVector<Polygon> &polygons_source = region.navmesh_polygons;
for (uint32_t n = 0; n < polygons_source.size(); n++) {
polygons_source[n].id = polygon_count;
polygon_count++;
@ -103,38 +105,38 @@ void NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild &r
r_build.polygon_count = polygon_count;
}
void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuild &r_build) {
gd::PerformanceData &performance_data = r_build.performance_data;
NavMapIteration *map_iteration = r_build.map_iteration;
void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuild3D &r_build) {
PerformanceData &performance_data = r_build.performance_data;
NavMapIteration3D *map_iteration = r_build.map_iteration;
int polygon_count = r_build.polygon_count;
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
// Group all edges per key.
connection_pairs_map.clear();
connection_pairs_map.reserve(polygon_count);
int free_edges_count = 0; // How many ConnectionPairs have only one Connection.
for (NavRegionIteration &region : map_iteration->region_iterations) {
for (NavRegionIteration3D &region : map_iteration->region_iterations) {
if (!region.get_enabled()) {
continue;
}
for (gd::Polygon &poly : region.navmesh_polygons) {
for (Polygon &poly : region.navmesh_polygons) {
for (uint32_t p = 0; p < poly.points.size(); p++) {
const int next_point = (p + 1) % poly.points.size();
const gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
const EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
HashMap<EdgeKey, EdgeConnectionPair, EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
if (!pair_it) {
pair_it = connection_pairs_map.insert(ek, gd::EdgeConnectionPair());
pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair());
performance_data.pm_edge_count += 1;
++free_edges_count;
}
gd::EdgeConnectionPair &pair = pair_it->value;
EdgeConnectionPair &pair = pair_it->value;
if (pair.size < 2) {
// Add the polygon/edge tuple to this key.
gd::Edge::Connection new_connection;
Edge::Connection new_connection;
new_connection.polygon = &poly;
new_connection.edge = p;
new_connection.pathway_start = poly.points[p].pos;
@ -157,23 +159,23 @@ void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuil
r_build.free_edge_count = free_edges_count;
}
void NavMapBuilder3D::_build_step_merge_edge_connection_pairs(NavMapIterationBuild &r_build) {
gd::PerformanceData &performance_data = r_build.performance_data;
void NavMapBuilder3D::_build_step_merge_edge_connection_pairs(NavMapIterationBuild3D &r_build) {
PerformanceData &performance_data = r_build.performance_data;
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
LocalVector<gd::Edge::Connection> &free_edges = r_build.iter_free_edges;
HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
LocalVector<Edge::Connection> &free_edges = r_build.iter_free_edges;
int free_edges_count = r_build.free_edge_count;
bool use_edge_connections = r_build.use_edge_connections;
free_edges.clear();
free_edges.reserve(free_edges_count);
for (const KeyValue<gd::EdgeKey, gd::EdgeConnectionPair> &pair_it : connection_pairs_map) {
const gd::EdgeConnectionPair &pair = pair_it.value;
for (const KeyValue<EdgeKey, EdgeConnectionPair> &pair_it : connection_pairs_map) {
const EdgeConnectionPair &pair = pair_it.value;
if (pair.size == 2) {
// Connect edge that are shared in different polygons.
const gd::Edge::Connection &c1 = pair.connections[0];
const gd::Edge::Connection &c2 = pair.connections[1];
const Edge::Connection &c1 = pair.connections[0];
const Edge::Connection &c2 = pair.connections[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.
@ -187,13 +189,13 @@ void NavMapBuilder3D::_build_step_merge_edge_connection_pairs(NavMapIterationBui
}
}
void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapIterationBuild &r_build) {
gd::PerformanceData &performance_data = r_build.performance_data;
NavMapIteration *map_iteration = r_build.map_iteration;
void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapIterationBuild3D &r_build) {
PerformanceData &performance_data = r_build.performance_data;
NavMapIteration3D *map_iteration = r_build.map_iteration;
real_t edge_connection_margin = r_build.edge_connection_margin;
LocalVector<gd::Edge::Connection> &free_edges = r_build.iter_free_edges;
HashMap<uint32_t, LocalVector<gd::Edge::Connection>> &region_external_connections = map_iteration->external_region_connections;
LocalVector<Edge::Connection> &free_edges = r_build.iter_free_edges;
HashMap<uint32_t, LocalVector<Edge::Connection>> &region_external_connections = map_iteration->external_region_connections;
// Find the compatible near edges.
//
@ -207,12 +209,12 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin;
for (uint32_t i = 0; i < free_edges.size(); i++) {
const gd::Edge::Connection &free_edge = free_edges[i];
const 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 (uint32_t j = 0; j < free_edges.size(); j++) {
const gd::Edge::Connection &other_edge = free_edges[j];
const Edge::Connection &other_edge = free_edges[j];
if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
continue;
}
@ -252,7 +254,7 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
}
// The edges can now be connected.
gd::Edge::Connection new_connection = other_edge;
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);
@ -264,14 +266,14 @@ void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapItera
}
}
void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_build) {
NavMapIteration *map_iteration = r_build.map_iteration;
void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild3D &r_build) {
NavMapIteration3D *map_iteration = r_build.map_iteration;
real_t link_connection_radius = r_build.link_connection_radius;
Vector3 merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size;
LocalVector<gd::Polygon> &link_polygons = map_iteration->link_polygons;
LocalVector<NavLinkIteration> &links = map_iteration->link_iterations;
LocalVector<Polygon> &link_polygons = map_iteration->link_polygons;
LocalVector<NavLinkIteration3D> &links = map_iteration->link_iterations;
int polygon_count = r_build.polygon_count;
real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius;
@ -279,22 +281,22 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
link_polygons.resize(links.size());
// Search for polygons within range of a nav link.
for (const NavLinkIteration &link : links) {
for (const NavLinkIteration3D &link : links) {
if (!link.get_enabled()) {
continue;
}
const Vector3 link_start_pos = link.get_start_position();
const Vector3 link_end_pos = link.get_end_position();
gd::Polygon *closest_start_polygon = nullptr;
Polygon *closest_start_polygon = nullptr;
real_t closest_start_sqr_dist = link_connection_radius_sqr;
Vector3 closest_start_point;
gd::Polygon *closest_end_polygon = nullptr;
Polygon *closest_end_polygon = nullptr;
real_t closest_end_sqr_dist = link_connection_radius_sqr;
Vector3 closest_end_point;
for (NavRegionIteration &region : map_iteration->region_iterations) {
for (NavRegionIteration3D &region : map_iteration->region_iterations) {
if (!region.get_enabled()) {
continue;
}
@ -303,8 +305,7 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
continue;
}
for (gd::Polygon &polyon : region.navmesh_polygons) {
//for (gd::Polygon &polyon : polygons) {
for (Polygon &polyon : region.navmesh_polygons) {
for (uint32_t point_id = 2; point_id < polyon.points.size(); point_id += 1) {
const Face3 face(polyon.points[0].pos, polyon.points[point_id - 1].pos, polyon.points[point_id].pos);
@ -337,7 +338,7 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
// If we have both a start and end point, then create a synthetic polygon to route through.
if (closest_start_polygon && closest_end_polygon) {
gd::Polygon &new_polygon = link_polygons[link_poly_idx++];
Polygon &new_polygon = link_polygons[link_poly_idx++];
new_polygon.id = polygon_count++;
new_polygon.owner = &link;
@ -353,14 +354,14 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
// Setup connections to go forward in the link.
{
gd::Edge::Connection entry_connection;
Edge::Connection entry_connection;
entry_connection.polygon = &new_polygon;
entry_connection.edge = -1;
entry_connection.pathway_start = new_polygon.points[0].pos;
entry_connection.pathway_end = new_polygon.points[1].pos;
closest_start_polygon->edges[0].connections.push_back(entry_connection);
gd::Edge::Connection exit_connection;
Edge::Connection exit_connection;
exit_connection.polygon = closest_end_polygon;
exit_connection.edge = -1;
exit_connection.pathway_start = new_polygon.points[2].pos;
@ -370,14 +371,14 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
// If the link is bi-directional, create connections from the end to the start.
if (link.is_bidirectional()) {
gd::Edge::Connection entry_connection;
Edge::Connection entry_connection;
entry_connection.polygon = &new_polygon;
entry_connection.edge = -1;
entry_connection.pathway_start = new_polygon.points[2].pos;
entry_connection.pathway_end = new_polygon.points[3].pos;
closest_end_polygon->edges[0].connections.push_back(entry_connection);
gd::Edge::Connection exit_connection;
Edge::Connection exit_connection;
exit_connection.polygon = closest_start_polygon;
exit_connection.edge = -1;
exit_connection.pathway_start = new_polygon.points[0].pos;
@ -388,10 +389,10 @@ void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_bu
}
}
void NavMapBuilder3D::_build_update_map_iteration(NavMapIterationBuild &r_build) {
NavMapIteration *map_iteration = r_build.map_iteration;
void NavMapBuilder3D::_build_update_map_iteration(NavMapIterationBuild3D &r_build) {
NavMapIteration3D *map_iteration = r_build.map_iteration;
LocalVector<gd::Polygon> &link_polygons = map_iteration->link_polygons;
LocalVector<Polygon> &link_polygons = map_iteration->link_polygons;
map_iteration->navmesh_polygon_count = r_build.polygon_count;
map_iteration->link_polygon_count = link_polygons.size();

20
modules/navigation/3d/nav_map_builder_3d.h
View File

@ -30,20 +30,20 @@
#pragma once
#include "../nav_utils.h"
#include "../nav_utils_3d.h"
struct NavMapIterationBuild;
struct NavMapIterationBuild3D;
class NavMapBuilder3D {
static void _build_step_gather_region_polygons(NavMapIterationBuild &r_build);
static void _build_step_find_edge_connection_pairs(NavMapIterationBuild &r_build);
static void _build_step_merge_edge_connection_pairs(NavMapIterationBuild &r_build);
static void _build_step_edge_connection_margin_connections(NavMapIterationBuild &r_build);
static void _build_step_navlink_connections(NavMapIterationBuild &r_build);
static void _build_update_map_iteration(NavMapIterationBuild &r_build);
static void _build_step_gather_region_polygons(NavMapIterationBuild3D &r_build);
static void _build_step_find_edge_connection_pairs(NavMapIterationBuild3D &r_build);
static void _build_step_merge_edge_connection_pairs(NavMapIterationBuild3D &r_build);
static void _build_step_edge_connection_margin_connections(NavMapIterationBuild3D &r_build);
static void _build_step_navlink_connections(NavMapIterationBuild3D &r_build);
static void _build_update_map_iteration(NavMapIterationBuild3D &r_build);
public:
static gd::PointKey get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size);
static nav_3d::PointKey get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size);
static void build_navmap_iteration(NavMapIterationBuild &r_build);
static void build_navmap_iteration(NavMapIterationBuild3D &r_build);
};

42
modules/navigation/3d/nav_map_iteration_3d.h
View File

@ -30,31 +30,31 @@
#pragma once
#include "../nav_rid.h"
#include "../nav_utils.h"
#include "../nav_rid_3d.h"
#include "../nav_utils_3d.h"
#include "nav_mesh_queries_3d.h"
#include "core/math/math_defs.h"
#include "core/os/semaphore.h"
struct NavLinkIteration;
class NavRegion;
struct NavRegionIteration;
struct NavMapIteration;
struct NavLinkIteration3D;
class NavRegion3D;
struct NavRegionIteration3D;
struct NavMapIteration3D;
struct NavMapIterationBuild {
struct NavMapIterationBuild3D {
Vector3 merge_rasterizer_cell_size;
bool use_edge_connections = true;
real_t edge_connection_margin;
real_t link_connection_radius;
gd::PerformanceData performance_data;
nav_3d::PerformanceData performance_data;
int polygon_count = 0;
int free_edge_count = 0;
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey> iter_connection_pairs_map;
LocalVector<gd::Edge::Connection> iter_free_edges;
HashMap<nav_3d::EdgeKey, nav_3d::EdgeConnectionPair, nav_3d::EdgeKey> iter_connection_pairs_map;
LocalVector<nav_3d::Edge::Connection> iter_free_edges;
NavMapIteration *map_iteration = nullptr;
NavMapIteration3D *map_iteration = nullptr;
int navmesh_polygon_count = 0;
int link_polygon_count = 0;
@ -72,39 +72,39 @@ struct NavMapIterationBuild {
}
};
struct NavMapIteration {
struct NavMapIteration3D {
mutable SafeNumeric<uint32_t> users;
RWLock rwlock;
Vector3 map_up;
LocalVector<gd::Polygon> link_polygons;
LocalVector<nav_3d::Polygon> link_polygons;
LocalVector<NavRegionIteration> region_iterations;
LocalVector<NavLinkIteration> link_iterations;
LocalVector<NavRegionIteration3D> region_iterations;
LocalVector<NavLinkIteration3D> link_iterations;
int navmesh_polygon_count = 0;
int link_polygon_count = 0;
// The edge connections that the map builds on top with the edge connection margin.
HashMap<uint32_t, LocalVector<gd::Edge::Connection>> external_region_connections;
HashMap<uint32_t, LocalVector<nav_3d::Edge::Connection>> external_region_connections;
HashMap<NavRegion *, uint32_t> region_ptr_to_region_id;
HashMap<NavRegion3D *, uint32_t> region_ptr_to_region_id;
LocalVector<NavMeshQueries3D::PathQuerySlot> path_query_slots;
Mutex path_query_slots_mutex;
Semaphore path_query_slots_semaphore;
};
class NavMapIterationRead {
const NavMapIteration &map_iteration;
class NavMapIterationRead3D {
const NavMapIteration3D &map_iteration;
public:
_ALWAYS_INLINE_ NavMapIterationRead(const NavMapIteration &p_iteration) :
_ALWAYS_INLINE_ NavMapIterationRead3D(const NavMapIteration3D &p_iteration) :
map_iteration(p_iteration) {
map_iteration.rwlock.read_lock();
map_iteration.users.increment();
}
_ALWAYS_INLINE_ ~NavMapIterationRead() {
_ALWAYS_INLINE_ ~NavMapIterationRead3D() {
map_iteration.users.decrement();
map_iteration.rwlock.read_unlock();
}

150
modules/navigation/3d/nav_mesh_queries_3d.cpp
View File

@ -32,13 +32,15 @@
#include "nav_mesh_queries_3d.h"
#include "../nav_base.h"
#include "../nav_map.h"
#include "../nav_base_3d.h"
#include "../nav_map_3d.h"
#include "nav_region_iteration_3d.h"
#include "core/math/geometry_3d.h"
#include "servers/navigation/navigation_utilities.h"
using namespace nav_3d;
#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
bool NavMeshQueries3D::emit_callback(const Callable &p_callback) {
@ -51,8 +53,8 @@ bool NavMeshQueries3D::emit_callback(const Callable &p_callback) {
return ce.error == Callable::CallError::CALL_OK;
}
Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly) {
const LocalVector<gd::Polygon> &region_polygons = p_polygons;
Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly) {
const LocalVector<Polygon> &region_polygons = p_polygons;
if (region_polygons.is_empty()) {
return Vector3();
@ -63,7 +65,7 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygo
RBMap<real_t, uint32_t> region_area_map;
for (uint32_t rp_index = 0; rp_index < region_polygons.size(); rp_index++) {
const gd::Polygon &region_polygon = region_polygons[rp_index];
const Polygon &region_polygon = region_polygons[rp_index];
real_t polyon_area = region_polygon.surface_area;
if (polyon_area == 0.0) {
@ -84,7 +86,7 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygo
uint32_t rrp_polygon_index = region_E->value;
ERR_FAIL_UNSIGNED_INDEX_V(rrp_polygon_index, region_polygons.size(), Vector3());
const gd::Polygon &rr_polygon = region_polygons[rrp_polygon_index];
const Polygon &rr_polygon = region_polygons[rrp_polygon_index];
real_t accumulated_polygon_area = 0;
RBMap<real_t, uint32_t> polygon_area_map;
@ -118,7 +120,7 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygo
} else {
uint32_t rrp_polygon_index = Math::random(int(0), region_polygons.size() - 1);
const gd::Polygon &rr_polygon = region_polygons[rrp_polygon_index];
const Polygon &rr_polygon = region_polygons[rrp_polygon_index];
uint32_t rrp_face_index = Math::random(int(2), rr_polygon.points.size() - 1);
@ -129,7 +131,7 @@ Vector3 NavMeshQueries3D::polygons_get_random_point(const LocalVector<gd::Polygo
}
}
void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const gd::Polygon *p_point_polygon) {
void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const Polygon *p_point_polygon) {
if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->get_type());
}
@ -145,7 +147,7 @@ void NavMeshQueries3D::_query_task_push_back_point_with_metadata(NavMeshPathQuer
p_query_task.path_points.push_back(p_point);
}
void NavMeshQueries3D::map_query_path(NavMap *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback) {
void NavMeshQueries3D::map_query_path(NavMap3D *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback) {
ERR_FAIL_NULL(map);
ERR_FAIL_COND(p_query_parameters.is_null());
ERR_FAIL_COND(p_query_result.is_null());
@ -229,13 +231,13 @@ void NavMeshQueries3D::map_query_path(NavMap *map, const Ref<NavigationPathQuery
}
}
void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration &p_map_iteration) {
void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration) {
real_t begin_d = FLT_MAX;
real_t end_d = FLT_MAX;
const LocalVector<NavRegionIteration> &regions = p_map_iteration.region_iterations;
const LocalVector<NavRegionIteration3D> &regions = p_map_iteration.region_iterations;
for (const NavRegionIteration &region : regions) {
for (const NavRegionIteration3D &region : regions) {
if (!region.get_enabled()) {
continue;
}
@ -248,7 +250,7 @@ void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask
}
// Find the initial poly and the end poly on this map.
for (const gd::Polygon &p : region.get_navmesh_polygons()) {
for (const Polygon &p : region.get_navmesh_polygons()) {
// Only consider the polygon if it in a region with compatible layers.
if ((p_query_task.navigation_layers & p.owner->get_navigation_layers()) == 0) {
continue;
@ -281,23 +283,23 @@ void NavMeshQueries3D::_query_task_find_start_end_positions(NavMeshPathQueryTask
void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task) {
const Vector3 p_target_position = p_query_task.target_position;
const uint32_t p_navigation_layers = p_query_task.navigation_layers;
const gd::Polygon *begin_poly = p_query_task.begin_polygon;
const gd::Polygon *end_poly = p_query_task.end_polygon;
const Polygon *begin_poly = p_query_task.begin_polygon;
const Polygon *end_poly = p_query_task.end_polygon;
Vector3 begin_point = p_query_task.begin_position;
Vector3 end_point = p_query_task.end_position;
// Heap of polygons to travel next.
gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer>
Heap<NavigationPoly *, NavPolyTravelCostGreaterThan, NavPolyHeapIndexer>
&traversable_polys = p_query_task.path_query_slot->traversable_polys;
traversable_polys.clear();
LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
for (gd::NavigationPoly &polygon : navigation_polys) {
LocalVector<NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
for (NavigationPoly &polygon : navigation_polys) {
polygon.reset();
}
// Initialize the matching navigation polygon.
gd::NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id];
NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id];
begin_navigation_poly.poly = begin_poly;
begin_navigation_poly.entry = begin_point;
begin_navigation_poly.back_navigation_edge_pathway_start = begin_point;
@ -308,23 +310,23 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
uint32_t least_cost_id = begin_poly->id;
bool found_route = false;
const gd::Polygon *reachable_end = nullptr;
const Polygon *reachable_end = nullptr;
real_t distance_to_reachable_end = FLT_MAX;
bool is_reachable = true;
real_t poly_enter_cost = 0.0;
while (true) {
const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id];
const NavigationPoly &least_cost_poly = navigation_polys[least_cost_id];
real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost();
// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
for (const gd::Edge &edge : least_cost_poly.poly->edges) {
for (const Edge &edge : least_cost_poly.poly->edges) {
// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
for (uint32_t connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
const gd::Edge::Connection &connection = edge.connections[connection_index];
const Edge::Connection &connection = edge.connections[connection_index];
// Only consider the connection to another polygon if this polygon is in a region with compatible layers.
const NavBaseIteration *owner = connection.polygon->owner;
const NavBaseIteration3D *owner = connection.polygon->owner;
bool skip_connection = false;
if (p_query_task.exclude_regions || p_query_task.include_regions) {
switch (owner->get_type()) {
@ -336,7 +338,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
}
} break;
case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK: {
const LocalVector<gd::Polygon> &link_polygons = owner->get_navmesh_polygons();
const LocalVector<Polygon> &link_polygons = owner->get_navmesh_polygons();
if (link_polygons.size() != 2) {
// Whatever this is, it is not a valid connected link.
skip_connection = true;
@ -366,7 +368,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
const real_t new_traveled_distance = least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost + poly_enter_cost + least_cost_poly.traveled_distance;
// Check if the neighbor polygon has already been processed.
gd::NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id];
NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id];
if (new_traveled_distance < neighbor_poly.traveled_distance) {
// Add the polygon to the heap of polygons to traverse next.
neighbor_poly.back_navigation_poly_id = least_cost_id;
@ -438,7 +440,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
return;
}
for (gd::NavigationPoly &nav_poly : navigation_polys) {
for (NavigationPoly &nav_poly : navigation_polys) {
nav_poly.poly = nullptr;
nav_poly.traveled_distance = FLT_MAX;
}
@ -500,7 +502,7 @@ void NavMeshQueries3D::_query_task_build_path_corridor(NavMeshPathQueryTask3D &p
}
}
void NavMeshQueries3D::query_task_map_iteration_get_path(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration &p_map_iteration) {
void NavMeshQueries3D::query_task_map_iteration_get_path(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration) {
p_query_task.path_clear();
_query_task_find_start_end_positions(p_query_task, p_map_iteration);
@ -609,15 +611,15 @@ void NavMeshQueries3D::_query_task_simplified_path_points(NavMeshPathQueryTask3D
void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task) {
Vector3 end_point = p_query_task.end_position;
const gd::Polygon *end_poly = p_query_task.end_polygon;
const Polygon *end_poly = p_query_task.end_polygon;
Vector3 begin_point = p_query_task.begin_position;
const gd::Polygon *begin_poly = p_query_task.begin_polygon;
const Polygon *begin_poly = p_query_task.begin_polygon;
uint32_t least_cost_id = p_query_task.least_cost_id;
LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
LocalVector<NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
Vector3 p_map_up = p_query_task.map_up;
// Set the apex poly/point to the end point
gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end };
const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_point, back_pathway);
@ -631,12 +633,12 @@ void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryT
Vector3 apex_point = end_point;
gd::NavigationPoly *left_poly = apex_poly;
NavigationPoly *left_poly = apex_poly;
Vector3 left_portal = apex_point;
gd::NavigationPoly *right_poly = apex_poly;
NavigationPoly *right_poly = apex_poly;
Vector3 right_portal = apex_point;
gd::NavigationPoly *p = apex_poly;
NavigationPoly *p = apex_poly;
_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
@ -705,11 +707,11 @@ void NavMeshQueries3D::_query_task_post_process_corridorfunnel(NavMeshPathQueryT
void NavMeshQueries3D::_query_task_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task) {
Vector3 end_point = p_query_task.end_position;
const gd::Polygon *end_poly = p_query_task.end_polygon;
const Polygon *end_poly = p_query_task.end_polygon;
Vector3 begin_point = p_query_task.begin_position;
const gd::Polygon *begin_poly = p_query_task.begin_polygon;
const Polygon *begin_poly = p_query_task.begin_polygon;
uint32_t least_cost_id = p_query_task.least_cost_id;
LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
LocalVector<NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
@ -734,11 +736,11 @@ void NavMeshQueries3D::_query_task_post_process_edgecentered(NavMeshPathQueryTas
void NavMeshQueries3D::_query_task_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task) {
Vector3 end_point = p_query_task.end_position;
const gd::Polygon *end_poly = p_query_task.end_polygon;
const Polygon *end_poly = p_query_task.end_polygon;
Vector3 begin_point = p_query_task.begin_position;
const gd::Polygon *begin_poly = p_query_task.begin_polygon;
const Polygon *begin_poly = p_query_task.begin_polygon;
uint32_t least_cost_id = p_query_task.least_cost_id;
LocalVector<gd::NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
LocalVector<NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
@ -753,14 +755,14 @@ void NavMeshQueries3D::_query_task_post_process_nopostprocessing(NavMeshPathQuer
_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
}
Vector3 NavMeshQueries3D::map_iteration_get_closest_point_to_segment(const NavMapIteration &p_map_iteration, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) {
Vector3 NavMeshQueries3D::map_iteration_get_closest_point_to_segment(const NavMapIteration3D &p_map_iteration, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) {
bool use_collision = p_use_collision;
Vector3 closest_point;
real_t closest_point_distance = FLT_MAX;
const LocalVector<NavRegionIteration> &regions = p_map_iteration.region_iterations;
for (const NavRegionIteration &region : regions) {
for (const gd::Polygon &polygon : region.get_navmesh_polygons()) {
const LocalVector<NavRegionIteration3D> &regions = p_map_iteration.region_iterations;
for (const NavRegionIteration3D &region : regions) {
for (const Polygon &polygon : region.get_navmesh_polygons()) {
// For each face check the distance to the segment.
for (size_t point_id = 2; point_id < polygon.points.size(); point_id += 1) {
const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos);
@ -819,28 +821,28 @@ Vector3 NavMeshQueries3D::map_iteration_get_closest_point_to_segment(const NavMa
return closest_point;
}
Vector3 NavMeshQueries3D::map_iteration_get_closest_point(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
gd::ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
Vector3 NavMeshQueries3D::map_iteration_get_closest_point(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point) {
ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
return cp.point;
}
Vector3 NavMeshQueries3D::map_iteration_get_closest_point_normal(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
gd::ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
Vector3 NavMeshQueries3D::map_iteration_get_closest_point_normal(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point) {
ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
return cp.normal;
}
RID NavMeshQueries3D::map_iteration_get_closest_point_owner(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
gd::ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
RID NavMeshQueries3D::map_iteration_get_closest_point_owner(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point) {
ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
return cp.owner;
}
gd::ClosestPointQueryResult NavMeshQueries3D::map_iteration_get_closest_point_info(const NavMapIteration &p_map_iteration, const Vector3 &p_point) {
gd::ClosestPointQueryResult result;
ClosestPointQueryResult NavMeshQueries3D::map_iteration_get_closest_point_info(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point) {
ClosestPointQueryResult result;
real_t closest_point_distance_squared = FLT_MAX;
const LocalVector<NavRegionIteration> &regions = p_map_iteration.region_iterations;
for (const NavRegionIteration &region : regions) {
for (const gd::Polygon &polygon : region.get_navmesh_polygons()) {
const LocalVector<NavRegionIteration3D> &regions = p_map_iteration.region_iterations;
for (const NavRegionIteration3D &region : regions) {
for (const Polygon &polygon : region.get_navmesh_polygons()) {
Vector3 plane_normal = (polygon.points[1].pos - polygon.points[0].pos).cross(polygon.points[2].pos - polygon.points[0].pos);
Vector3 closest_on_polygon;
real_t closest = FLT_MAX;
@ -908,7 +910,7 @@ gd::ClosestPointQueryResult NavMeshQueries3D::map_iteration_get_closest_point_in
return result;
}
Vector3 NavMeshQueries3D::map_iteration_get_random_point(const NavMapIteration &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly) {
Vector3 NavMeshQueries3D::map_iteration_get_random_point(const NavMapIteration3D &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly) {
if (p_map_iteration.region_iterations.is_empty()) {
return Vector3();
}
@ -917,7 +919,7 @@ Vector3 NavMeshQueries3D::map_iteration_get_random_point(const NavMapIteration &
accessible_regions.reserve(p_map_iteration.region_iterations.size());
for (uint32_t i = 0; i < p_map_iteration.region_iterations.size(); i++) {
const NavRegionIteration &region = p_map_iteration.region_iterations[i];
const NavRegionIteration3D &region = p_map_iteration.region_iterations[i];
if (!region.enabled || (p_navigation_layers & region.navigation_layers) == 0) {
continue;
}
@ -934,7 +936,7 @@ Vector3 NavMeshQueries3D::map_iteration_get_random_point(const NavMapIteration &
RBMap<real_t, uint32_t> accessible_regions_area_map;
for (uint32_t accessible_region_index = 0; accessible_region_index < accessible_regions.size(); accessible_region_index++) {
const NavRegionIteration &region = p_map_iteration.region_iterations[accessible_regions[accessible_region_index]];
const NavRegionIteration3D &region = p_map_iteration.region_iterations[accessible_regions[accessible_region_index]];
real_t region_surface_area = region.surface_area;
@ -957,25 +959,25 @@ Vector3 NavMeshQueries3D::map_iteration_get_random_point(const NavMapIteration &
uint32_t random_region_index = E->value;
ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector3());
const NavRegionIteration &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
const NavRegionIteration3D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
} else {
uint32_t random_region_index = Math::random(int(0), accessible_regions.size() - 1);
const NavRegionIteration &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
const NavRegionIteration3D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
return NavMeshQueries3D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
}
}
Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) {
Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVector<Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) {
bool use_collision = p_use_collision;
Vector3 closest_point;
real_t closest_point_distance = FLT_MAX;
for (const gd::Polygon &polygon : p_polygons) {
for (const Polygon &polygon : p_polygons) {
// For each face check the distance to the segment.
for (size_t point_id = 2; point_id < polygon.points.size(); point_id += 1) {
const Face3 face(polygon.points[0].pos, polygon.points[point_id - 1].pos, polygon.points[point_id].pos);
@ -1033,21 +1035,21 @@ Vector3 NavMeshQueries3D::polygons_get_closest_point_to_segment(const LocalVecto
return closest_point;
}
Vector3 NavMeshQueries3D::polygons_get_closest_point(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point) {
gd::ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point);
Vector3 NavMeshQueries3D::polygons_get_closest_point(const LocalVector<Polygon> &p_polygons, const Vector3 &p_point) {
ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point);
return cp.point;
}
Vector3 NavMeshQueries3D::polygons_get_closest_point_normal(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point) {
gd::ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point);
Vector3 NavMeshQueries3D::polygons_get_closest_point_normal(const LocalVector<Polygon> &p_polygons, const Vector3 &p_point) {
ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point);
return cp.normal;
}
gd::ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point) {
gd::ClosestPointQueryResult result;
ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(const LocalVector<Polygon> &p_polygons, const Vector3 &p_point) {
ClosestPointQueryResult result;
real_t closest_point_distance_squared = FLT_MAX;
for (const gd::Polygon &polygon : p_polygons) {
for (const Polygon &polygon : p_polygons) {
Vector3 plane_normal = (polygon.points[1].pos - polygon.points[0].pos).cross(polygon.points[2].pos - polygon.points[0].pos);
Vector3 closest_on_polygon;
real_t closest = FLT_MAX;
@ -1114,14 +1116,14 @@ gd::ClosestPointQueryResult NavMeshQueries3D::polygons_get_closest_point_info(co
return result;
}
RID NavMeshQueries3D::polygons_get_closest_point_owner(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point) {
gd::ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point);
RID NavMeshQueries3D::polygons_get_closest_point_owner(const LocalVector<Polygon> &p_polygons, const Vector3 &p_point) {
ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point);
return cp.owner;
}
void NavMeshQueries3D::_query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) {
void NavMeshQueries3D::_query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const NavigationPoly *from_poly, const Vector3 &p_to_point, const NavigationPoly *p_to_poly) {
Vector3 from = p_query_task.path_points[p_query_task.path_points.size() - 1];
const LocalVector<gd::NavigationPoly> &p_navigation_polys = p_query_task.path_query_slot->path_corridor;
const LocalVector<NavigationPoly> &p_navigation_polys = p_query_task.path_query_slot->path_corridor;
const Vector3 p_map_up = p_query_task.map_up;
if (from.is_equal_approx(p_to_point)) {

50
modules/navigation/3d/nav_mesh_queries_3d.h
View File

@ -32,7 +32,7 @@
#ifndef _3D_DISABLED
#include "../nav_utils.h"
#include "../nav_utils_3d.h"
#include "servers/navigation/navigation_path_query_parameters_3d.h"
#include "servers/navigation/navigation_path_query_result_3d.h"
@ -40,14 +40,14 @@
using namespace NavigationUtilities;
class NavMap;
struct NavMapIteration;
class NavMap3D;
struct NavMapIteration3D;
class NavMeshQueries3D {
public:
struct PathQuerySlot {
LocalVector<gd::NavigationPoly> path_corridor;
gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer> traversable_polys;
LocalVector<nav_3d::NavigationPoly> path_corridor;
Heap<nav_3d::NavigationPoly *, nav_3d::NavPolyTravelCostGreaterThan, nav_3d::NavPolyHeapIndexer> traversable_polys;
bool in_use = false;
uint32_t slot_index = 0;
};
@ -78,13 +78,13 @@ public:
// Path building.
Vector3 begin_position;
Vector3 end_position;
const gd::Polygon *begin_polygon = nullptr;
const gd::Polygon *end_polygon = nullptr;
const nav_3d::Polygon *begin_polygon = nullptr;
const nav_3d::Polygon *end_polygon = nullptr;
uint32_t least_cost_id = 0;
// Map.
Vector3 map_up;
NavMap *map = nullptr;
NavMap3D *map = nullptr;
PathQuerySlot *path_query_slot = nullptr;
// Path points.
@ -115,31 +115,31 @@ public:
static bool emit_callback(const Callable &p_callback);
static Vector3 polygons_get_random_point(const LocalVector<gd::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly);
static Vector3 polygons_get_random_point(const LocalVector<nav_3d::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly);
static Vector3 polygons_get_closest_point_to_segment(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
static Vector3 polygons_get_closest_point(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
static Vector3 polygons_get_closest_point_normal(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
static gd::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
static RID polygons_get_closest_point_owner(const LocalVector<gd::Polygon> &p_polygons, const Vector3 &p_point);
static Vector3 polygons_get_closest_point_to_segment(const LocalVector<nav_3d::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
static Vector3 polygons_get_closest_point(const LocalVector<nav_3d::Polygon> &p_polygons, const Vector3 &p_point);
static Vector3 polygons_get_closest_point_normal(const LocalVector<nav_3d::Polygon> &p_polygons, const Vector3 &p_point);
static nav_3d::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector<nav_3d::Polygon> &p_polygons, const Vector3 &p_point);
static RID polygons_get_closest_point_owner(const LocalVector<nav_3d::Polygon> &p_polygons, const Vector3 &p_point);
static Vector3 map_iteration_get_closest_point_to_segment(const NavMapIteration &p_map_iteration, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
static Vector3 map_iteration_get_closest_point(const NavMapIteration &p_map_iteration, const Vector3 &p_point);
static Vector3 map_iteration_get_closest_point_normal(const NavMapIteration &p_map_iteration, const Vector3 &p_point);
static RID map_iteration_get_closest_point_owner(const NavMapIteration &p_map_iteration, const Vector3 &p_point);
static gd::ClosestPointQueryResult map_iteration_get_closest_point_info(const NavMapIteration &p_map_iteration, const Vector3 &p_point);
static Vector3 map_iteration_get_random_point(const NavMapIteration &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly);
static Vector3 map_iteration_get_closest_point_to_segment(const NavMapIteration3D &p_map_iteration, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
static Vector3 map_iteration_get_closest_point(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
static Vector3 map_iteration_get_closest_point_normal(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
static RID map_iteration_get_closest_point_owner(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
static nav_3d::ClosestPointQueryResult map_iteration_get_closest_point_info(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
static Vector3 map_iteration_get_random_point(const NavMapIteration3D &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly);
static void map_query_path(NavMap *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback);
static void map_query_path(NavMap3D *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback);
static void query_task_map_iteration_get_path(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration &p_map_iteration);
static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const gd::Polygon *p_point_polygon);
static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration &p_map_iteration);
static void query_task_map_iteration_get_path(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration);
static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const nav_3d::Polygon *p_point_polygon);
static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration);
static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task);
static void _query_task_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task);
static void _query_task_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task);
static void _query_task_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task);
static void _query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly);
static void _query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const nav_3d::NavigationPoly *from_poly, const Vector3 &p_to_point, const nav_3d::NavigationPoly *p_to_poly);
static void _query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task);
static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<uint32_t> &r_simplified_path_indices);

4
modules/navigation/3d/nav_region_iteration_3d.h
View File

@ -30,12 +30,12 @@
#pragma once
#include "../nav_utils.h"
#include "../nav_utils_3d.h"
#include "nav_base_iteration_3d.h"
#include "core/math/aabb.h"
struct NavRegionIteration : NavBaseIteration {
struct NavRegionIteration3D : NavBaseIteration3D {
Transform3D transform;
real_t surface_area = 0.0;
AABB bounds;

70
modules/navigation/nav_agent.cpp → modules/navigation/nav_agent_3d.cpp
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_agent.cpp */
/* nav_agent_3d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -28,21 +28,21 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "nav_agent.h"
#include "nav_agent_3d.h"
#include "nav_map.h"
#include "nav_map_3d.h"
void NavAgent::set_avoidance_enabled(bool p_enabled) {
void NavAgent3D::set_avoidance_enabled(bool p_enabled) {
avoidance_enabled = p_enabled;
_update_rvo_agent_properties();
}
void NavAgent::set_use_3d_avoidance(bool p_enabled) {
void NavAgent3D::set_use_3d_avoidance(bool p_enabled) {
use_3d_avoidance = p_enabled;
_update_rvo_agent_properties();
}
void NavAgent::_update_rvo_agent_properties() {
void NavAgent3D::_update_rvo_agent_properties() {
if (use_3d_avoidance) {
rvo_agent_3d.neighborDist_ = neighbor_distance;
rvo_agent_3d.maxNeighbors_ = max_neighbors;
@ -88,7 +88,7 @@ void NavAgent::_update_rvo_agent_properties() {
request_sync();
}
void NavAgent::set_map(NavMap *p_map) {
void NavAgent3D::set_map(NavMap3D *p_map) {
if (map == p_map) {
return;
}
@ -112,7 +112,7 @@ void NavAgent::set_map(NavMap *p_map) {
}
}
bool NavAgent::is_map_changed() {
bool NavAgent3D::is_map_changed() {
if (map) {
bool is_changed = map->get_iteration_id() != last_map_iteration_id;
last_map_iteration_id = map->get_iteration_id();
@ -122,15 +122,15 @@ bool NavAgent::is_map_changed() {
}
}
void NavAgent::set_avoidance_callback(Callable p_callback) {
void NavAgent3D::set_avoidance_callback(Callable p_callback) {
avoidance_callback = p_callback;
}
bool NavAgent::has_avoidance_callback() const {
bool NavAgent3D::has_avoidance_callback() const {
return avoidance_callback.is_valid();
}
void NavAgent::dispatch_avoidance_callback() {
void NavAgent3D::dispatch_avoidance_callback() {
if (!avoidance_callback.is_valid()) {
return;
}
@ -151,7 +151,7 @@ void NavAgent::dispatch_avoidance_callback() {
avoidance_callback.call(new_velocity);
}
void NavAgent::set_neighbor_distance(real_t p_neighbor_distance) {
void NavAgent3D::set_neighbor_distance(real_t p_neighbor_distance) {
neighbor_distance = p_neighbor_distance;
if (use_3d_avoidance) {
rvo_agent_3d.neighborDist_ = neighbor_distance;
@ -163,7 +163,7 @@ void NavAgent::set_neighbor_distance(real_t p_neighbor_distance) {
request_sync();
}
void NavAgent::set_max_neighbors(int p_max_neighbors) {
void NavAgent3D::set_max_neighbors(int p_max_neighbors) {
max_neighbors = p_max_neighbors;
if (use_3d_avoidance) {
rvo_agent_3d.maxNeighbors_ = max_neighbors;
@ -175,7 +175,7 @@ void NavAgent::set_max_neighbors(int p_max_neighbors) {
request_sync();
}
void NavAgent::set_time_horizon_agents(real_t p_time_horizon) {
void NavAgent3D::set_time_horizon_agents(real_t p_time_horizon) {
time_horizon_agents = p_time_horizon;
if (use_3d_avoidance) {
rvo_agent_3d.timeHorizon_ = time_horizon_agents;
@ -187,7 +187,7 @@ void NavAgent::set_time_horizon_agents(real_t p_time_horizon) {
request_sync();
}
void NavAgent::set_time_horizon_obstacles(real_t p_time_horizon) {
void NavAgent3D::set_time_horizon_obstacles(real_t p_time_horizon) {
time_horizon_obstacles = p_time_horizon;
if (use_3d_avoidance) {
rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles;
@ -199,7 +199,7 @@ void NavAgent::set_time_horizon_obstacles(real_t p_time_horizon) {
request_sync();
}
void NavAgent::set_radius(real_t p_radius) {
void NavAgent3D::set_radius(real_t p_radius) {
radius = p_radius;
if (use_3d_avoidance) {
rvo_agent_3d.radius_ = radius;
@ -211,7 +211,7 @@ void NavAgent::set_radius(real_t p_radius) {
request_sync();
}
void NavAgent::set_height(real_t p_height) {
void NavAgent3D::set_height(real_t p_height) {
height = p_height;
if (use_3d_avoidance) {
rvo_agent_3d.height_ = height;
@ -223,7 +223,7 @@ void NavAgent::set_height(real_t p_height) {
request_sync();
}
void NavAgent::set_max_speed(real_t p_max_speed) {
void NavAgent3D::set_max_speed(real_t p_max_speed) {
max_speed = p_max_speed;
if (avoidance_enabled) {
if (use_3d_avoidance) {
@ -237,7 +237,7 @@ void NavAgent::set_max_speed(real_t p_max_speed) {
request_sync();
}
void NavAgent::set_position(const Vector3 p_position) {
void NavAgent3D::set_position(const Vector3 p_position) {
position = p_position;
if (avoidance_enabled) {
if (use_3d_avoidance) {
@ -252,11 +252,11 @@ void NavAgent::set_position(const Vector3 p_position) {
request_sync();
}
void NavAgent::set_target_position(const Vector3 p_target_position) {
void NavAgent3D::set_target_position(const Vector3 p_target_position) {
target_position = p_target_position;
}
void NavAgent::set_velocity(const Vector3 p_velocity) {
void NavAgent3D::set_velocity(const Vector3 p_velocity) {
// Sets the "wanted" velocity for an agent as a suggestion
// This velocity is not guaranteed, RVO simulation will only try to fulfill it
velocity = p_velocity;
@ -272,7 +272,7 @@ void NavAgent::set_velocity(const Vector3 p_velocity) {
request_sync();
}
void NavAgent::set_velocity_forced(const Vector3 p_velocity) {
void NavAgent3D::set_velocity_forced(const Vector3 p_velocity) {
// This function replaces the internal rvo simulation velocity
// should only be used after the agent was teleported
// as it destroys consistency in movement in cramped situations
@ -290,7 +290,7 @@ void NavAgent::set_velocity_forced(const Vector3 p_velocity) {
request_sync();
}
void NavAgent::update() {
void NavAgent3D::update() {
// Updates this agent with the calculated results from the rvo simulation
if (avoidance_enabled) {
if (use_3d_avoidance) {
@ -301,7 +301,7 @@ void NavAgent::update() {
}
}
void NavAgent::set_avoidance_mask(uint32_t p_mask) {
void NavAgent3D::set_avoidance_mask(uint32_t p_mask) {
avoidance_mask = p_mask;
if (use_3d_avoidance) {
rvo_agent_3d.avoidance_mask_ = avoidance_mask;
@ -313,7 +313,7 @@ void NavAgent::set_avoidance_mask(uint32_t p_mask) {
request_sync();
}
void NavAgent::set_avoidance_layers(uint32_t p_layers) {
void NavAgent3D::set_avoidance_layers(uint32_t p_layers) {
avoidance_layers = p_layers;
if (use_3d_avoidance) {
rvo_agent_3d.avoidance_layers_ = avoidance_layers;
@ -325,7 +325,7 @@ void NavAgent::set_avoidance_layers(uint32_t p_layers) {
request_sync();
}
void NavAgent::set_avoidance_priority(real_t p_priority) {
void NavAgent3D::set_avoidance_priority(real_t p_priority) {
ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
avoidance_priority = p_priority;
@ -339,15 +339,15 @@ void NavAgent::set_avoidance_priority(real_t p_priority) {
request_sync();
}
bool NavAgent::is_dirty() const {
bool NavAgent3D::is_dirty() const {
return agent_dirty;
}
void NavAgent::sync() {
void NavAgent3D::sync() {
agent_dirty = false;
}
const Dictionary NavAgent::get_avoidance_data() const {
const Dictionary NavAgent3D::get_avoidance_data() const {
// Returns debug data from RVO simulation internals of this agent.
Dictionary _avoidance_data;
if (use_3d_avoidance) {
@ -384,7 +384,7 @@ const Dictionary NavAgent::get_avoidance_data() const {
return _avoidance_data;
}
void NavAgent::set_paused(bool p_paused) {
void NavAgent3D::set_paused(bool p_paused) {
if (paused == p_paused) {
return;
}
@ -400,26 +400,26 @@ void NavAgent::set_paused(bool p_paused) {
}
}
bool NavAgent::get_paused() const {
bool NavAgent3D::get_paused() const {
return paused;
}
void NavAgent::request_sync() {
void NavAgent3D::request_sync() {
if (map && !sync_dirty_request_list_element.in_list()) {
map->add_agent_sync_dirty_request(&sync_dirty_request_list_element);
}
}
void NavAgent::cancel_sync_request() {
void NavAgent3D::cancel_sync_request() {
if (map && sync_dirty_request_list_element.in_list()) {
map->remove_agent_sync_dirty_request(&sync_dirty_request_list_element);
}
}
NavAgent::NavAgent() :
NavAgent3D::NavAgent3D() :
sync_dirty_request_list_element(this) {
}
NavAgent::~NavAgent() {
NavAgent3D::~NavAgent3D() {
cancel_sync_request();
}

20
modules/navigation/nav_agent.h → modules/navigation/nav_agent_3d.h
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_agent.h */
/* nav_agent_3d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -30,7 +30,7 @@
#pragma once
#include "nav_rid.h"
#include "nav_rid_3d.h"
#include "core/object/class_db.h"
#include "core/templates/self_list.h"
@ -38,9 +38,9 @@
#include <Agent2d.h>
#include <Agent3d.h>
class NavMap;
class NavMap3D;
class NavAgent : public NavRid {
class NavAgent3D : public NavRid3D {
Vector3 position;
Vector3 target_position;
Vector3 velocity;
@ -55,7 +55,7 @@ class NavAgent : public NavRid {
Vector3 safe_velocity;
bool clamp_speed = true; // Experimental, clamps velocity to max_speed.
NavMap *map = nullptr;
NavMap3D *map = nullptr;
RVO2D::Agent2D rvo_agent_2d;
RVO3D::Agent3D rvo_agent_3d;
@ -73,11 +73,11 @@ class NavAgent : public NavRid {
uint32_t last_map_iteration_id = 0;
bool paused = false;
SelfList<NavAgent> sync_dirty_request_list_element;
SelfList<NavAgent3D> sync_dirty_request_list_element;
public:
NavAgent();
~NavAgent();
NavAgent3D();
~NavAgent3D();
void set_avoidance_enabled(bool p_enabled);
bool is_avoidance_enabled() { return avoidance_enabled; }
@ -85,8 +85,8 @@ public:
void set_use_3d_avoidance(bool p_enabled);
bool get_use_3d_avoidance() { return use_3d_avoidance; }
void set_map(NavMap *p_map);
NavMap *get_map() { return map; }
void set_map(NavMap3D *p_map);
NavMap3D *get_map() { return map; }
bool is_map_changed();

12
modules/navigation/nav_base.h → modules/navigation/nav_base_3d.h
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_base.h */
/* nav_base_3d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -30,14 +30,14 @@
#pragma once
#include "nav_rid.h"
#include "nav_utils.h"
#include "nav_rid_3d.h"
#include "nav_utils_3d.h"
#include "servers/navigation/navigation_utilities.h"
class NavMap;
class NavMap3D;
class NavBase : public NavRid {
class NavBase3D : public NavRid3D {
protected:
uint32_t navigation_layers = 1;
real_t enter_cost = 0.0;
@ -63,5 +63,5 @@ public:
virtual void set_owner_id(ObjectID p_owner_id) {}
ObjectID get_owner_id() const { return owner_id; }
virtual ~NavBase() {}
virtual ~NavBase3D() {}
};

38
modules/navigation/nav_link.cpp → modules/navigation/nav_link_3d.cpp
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_link.cpp */
/* nav_link_3d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -28,11 +28,11 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "nav_link.h"
#include "nav_link_3d.h"
#include "nav_map.h"
#include "nav_map_3d.h"
void NavLink::set_map(NavMap *p_map) {
void NavLink3D::set_map(NavMap3D *p_map) {
if (map == p_map) {
return;
}
@ -52,7 +52,7 @@ void NavLink::set_map(NavMap *p_map) {
}
}
void NavLink::set_enabled(bool p_enabled) {
void NavLink3D::set_enabled(bool p_enabled) {
if (enabled == p_enabled) {
return;
}
@ -64,7 +64,7 @@ void NavLink::set_enabled(bool p_enabled) {
request_sync();
}
void NavLink::set_bidirectional(bool p_bidirectional) {
void NavLink3D::set_bidirectional(bool p_bidirectional) {
if (bidirectional == p_bidirectional) {
return;
}
@ -74,7 +74,7 @@ void NavLink::set_bidirectional(bool p_bidirectional) {
request_sync();
}
void NavLink::set_start_position(const Vector3 p_position) {
void NavLink3D::set_start_position(const Vector3 p_position) {
if (start_position == p_position) {
return;
}
@ -84,7 +84,7 @@ void NavLink::set_start_position(const Vector3 p_position) {
request_sync();
}
void NavLink::set_end_position(const Vector3 p_position) {
void NavLink3D::set_end_position(const Vector3 p_position) {
if (end_position == p_position) {
return;
}
@ -94,7 +94,7 @@ void NavLink::set_end_position(const Vector3 p_position) {
request_sync();
}
void NavLink::set_navigation_layers(uint32_t p_navigation_layers) {
void NavLink3D::set_navigation_layers(uint32_t p_navigation_layers) {
if (navigation_layers == p_navigation_layers) {
return;
}
@ -104,7 +104,7 @@ void NavLink::set_navigation_layers(uint32_t p_navigation_layers) {
request_sync();
}
void NavLink::set_enter_cost(real_t p_enter_cost) {
void NavLink3D::set_enter_cost(real_t p_enter_cost) {
real_t new_enter_cost = MAX(p_enter_cost, 0.0);
if (enter_cost == new_enter_cost) {
return;
@ -115,7 +115,7 @@ void NavLink::set_enter_cost(real_t p_enter_cost) {
request_sync();
}
void NavLink::set_travel_cost(real_t p_travel_cost) {
void NavLink3D::set_travel_cost(real_t p_travel_cost) {
real_t new_travel_cost = MAX(p_travel_cost, 0.0);
if (travel_cost == new_travel_cost) {
return;
@ -126,7 +126,7 @@ void NavLink::set_travel_cost(real_t p_travel_cost) {
request_sync();
}
void NavLink::set_owner_id(ObjectID p_owner_id) {
void NavLink3D::set_owner_id(ObjectID p_owner_id) {
if (owner_id == p_owner_id) {
return;
}
@ -136,36 +136,36 @@ void NavLink::set_owner_id(ObjectID p_owner_id) {
request_sync();
}
bool NavLink::is_dirty() const {
bool NavLink3D::is_dirty() const {
return link_dirty;
}
void NavLink::sync() {
void NavLink3D::sync() {
link_dirty = false;
}
void NavLink::request_sync() {
void NavLink3D::request_sync() {
if (map && !sync_dirty_request_list_element.in_list()) {
map->add_link_sync_dirty_request(&sync_dirty_request_list_element);
}
}
void NavLink::cancel_sync_request() {
void NavLink3D::cancel_sync_request() {
if (map && sync_dirty_request_list_element.in_list()) {
map->remove_link_sync_dirty_request(&sync_dirty_request_list_element);
}
}
NavLink::NavLink() :
NavLink3D::NavLink3D() :
sync_dirty_request_list_element(this) {
type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK;
}
NavLink::~NavLink() {
NavLink3D::~NavLink3D() {
cancel_sync_request();
}
void NavLink::get_iteration_update(NavLinkIteration &r_iteration) {
void NavLink3D::get_iteration_update(NavLinkIteration3D &r_iteration) {
r_iteration.navigation_layers = get_navigation_layers();
r_iteration.enter_cost = get_enter_cost();
r_iteration.travel_cost = get_travel_cost();

24
modules/navigation/nav_link.h → modules/navigation/nav_link_3d.h
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_link.h */
/* nav_link_3d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -31,10 +31,10 @@
#pragma once
#include "3d/nav_base_iteration_3d.h"
#include "nav_base.h"
#include "nav_utils.h"
#include "nav_base_3d.h"
#include "nav_utils_3d.h"
struct NavLinkIteration : NavBaseIteration {
struct NavLinkIteration3D : NavBaseIteration3D {
bool bidirectional = true;
Vector3 start_position;
Vector3 end_position;
@ -46,8 +46,8 @@ struct NavLinkIteration : NavBaseIteration {
#include "core/templates/self_list.h"
class NavLink : public NavBase {
NavMap *map = nullptr;
class NavLink3D : public NavBase3D {
NavMap3D *map = nullptr;
bool bidirectional = true;
Vector3 start_position;
Vector3 end_position;
@ -55,14 +55,14 @@ class NavLink : public NavBase {
bool link_dirty = true;
SelfList<NavLink> sync_dirty_request_list_element;
SelfList<NavLink3D> sync_dirty_request_list_element;
public:
NavLink();
~NavLink();
NavLink3D();
~NavLink3D();
void set_map(NavMap *p_map);
NavMap *get_map() const {
void set_map(NavMap3D *p_map);
NavMap3D *get_map() const {
return map;
}
@ -95,5 +95,5 @@ public:
void request_sync();
void cancel_sync_request();
void get_iteration_update(NavLinkIteration &r_iteration);
void get_iteration_update(NavLinkIteration3D &r_iteration);
};

214
modules/navigation/nav_map.cpp → modules/navigation/nav_map_3d.cpp
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_map.cpp */
/* nav_map_3d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -28,21 +28,23 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "nav_map.h"
#include "nav_map_3d.h"
#include "3d/nav_map_builder_3d.h"
#include "3d/nav_mesh_queries_3d.h"
#include "3d/nav_region_iteration_3d.h"
#include "nav_agent.h"
#include "nav_link.h"
#include "nav_obstacle.h"
#include "nav_region.h"
#include "nav_agent_3d.h"
#include "nav_link_3d.h"
#include "nav_obstacle_3d.h"
#include "nav_region_3d.h"
#include "core/config/project_settings.h"
#include "core/object/worker_thread_pool.h"
#include <Obstacle2d.h>
using namespace nav_3d;
#ifdef DEBUG_ENABLED
#define NAVMAP_ITERATION_ZERO_ERROR_MSG() \
ERR_PRINT_ONCE("NavigationServer navigation map query failed because it was made before first map synchronization.\n\
@ -52,19 +54,19 @@
#define NAVMAP_ITERATION_ZERO_ERROR_MSG()
#endif // DEBUG_ENABLED
#define GET_MAP_ITERATION() \
iteration_slot_rwlock.read_lock(); \
NavMapIteration &map_iteration = iteration_slots[iteration_slot_index]; \
NavMapIterationRead iteration_read_lock(map_iteration); \
#define GET_MAP_ITERATION() \
iteration_slot_rwlock.read_lock(); \
NavMapIteration3D &map_iteration = iteration_slots[iteration_slot_index]; \
NavMapIterationRead3D iteration_read_lock(map_iteration); \
iteration_slot_rwlock.read_unlock();
#define GET_MAP_ITERATION_CONST() \
iteration_slot_rwlock.read_lock(); \
const NavMapIteration &map_iteration = iteration_slots[iteration_slot_index]; \
NavMapIterationRead iteration_read_lock(map_iteration); \
#define GET_MAP_ITERATION_CONST() \
iteration_slot_rwlock.read_lock(); \
const NavMapIteration3D &map_iteration = iteration_slots[iteration_slot_index]; \
NavMapIterationRead3D iteration_read_lock(map_iteration); \
iteration_slot_rwlock.read_unlock();
void NavMap::set_up(Vector3 p_up) {
void NavMap3D::set_up(Vector3 p_up) {
if (up == p_up) {
return;
}
@ -72,7 +74,7 @@ void NavMap::set_up(Vector3 p_up) {
map_settings_dirty = true;
}
void NavMap::set_cell_size(real_t p_cell_size) {
void NavMap3D::set_cell_size(real_t p_cell_size) {
if (cell_size == p_cell_size) {
return;
}
@ -81,7 +83,7 @@ void NavMap::set_cell_size(real_t p_cell_size) {
map_settings_dirty = true;
}
void NavMap::set_cell_height(real_t p_cell_height) {
void NavMap3D::set_cell_height(real_t p_cell_height) {
if (cell_height == p_cell_height) {
return;
}
@ -90,7 +92,7 @@ void NavMap::set_cell_height(real_t p_cell_height) {
map_settings_dirty = true;
}
void NavMap::set_merge_rasterizer_cell_scale(float p_value) {
void NavMap3D::set_merge_rasterizer_cell_scale(float p_value) {
if (merge_rasterizer_cell_scale == p_value) {
return;
}
@ -99,7 +101,7 @@ void NavMap::set_merge_rasterizer_cell_scale(float p_value) {
map_settings_dirty = true;
}
void NavMap::set_use_edge_connections(bool p_enabled) {
void NavMap3D::set_use_edge_connections(bool p_enabled) {
if (use_edge_connections == p_enabled) {
return;
}
@ -107,7 +109,7 @@ void NavMap::set_use_edge_connections(bool p_enabled) {
iteration_dirty = true;
}
void NavMap::set_edge_connection_margin(real_t p_edge_connection_margin) {
void NavMap3D::set_edge_connection_margin(real_t p_edge_connection_margin) {
if (edge_connection_margin == p_edge_connection_margin) {
return;
}
@ -115,7 +117,7 @@ void NavMap::set_edge_connection_margin(real_t p_edge_connection_margin) {
iteration_dirty = true;
}
void NavMap::set_link_connection_radius(real_t p_link_connection_radius) {
void NavMap3D::set_link_connection_radius(real_t p_link_connection_radius) {
if (link_connection_radius == p_link_connection_radius) {
return;
}
@ -123,16 +125,16 @@ void NavMap::set_link_connection_radius(real_t p_link_connection_radius) {
iteration_dirty = true;
}
const Vector3 &NavMap::get_merge_rasterizer_cell_size() const {
const Vector3 &NavMap3D::get_merge_rasterizer_cell_size() const {
return merge_rasterizer_cell_size;
}
gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
PointKey NavMap3D::get_point_key(const Vector3 &p_pos) const {
const int x = static_cast<int>(Math::floor(p_pos.x / merge_rasterizer_cell_size.x));
const int y = static_cast<int>(Math::floor(p_pos.y / merge_rasterizer_cell_size.y));
const int z = static_cast<int>(Math::floor(p_pos.z / merge_rasterizer_cell_size.z));
gd::PointKey p;
PointKey p;
p.key = 0;
p.x = x;
p.y = y;
@ -140,7 +142,7 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
return p;
}
void NavMap::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task) {
void NavMap3D::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task) {
if (iteration_id == 0) {
return;
}
@ -161,7 +163,7 @@ void NavMap::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task)
if (p_query_task.path_query_slot == nullptr) {
map_iteration.path_query_slots_semaphore.post();
ERR_FAIL_NULL_MSG(p_query_task.path_query_slot, "No unused NavMap path query slot found! This should never happen :(.");
ERR_FAIL_NULL_MSG(p_query_task.path_query_slot, "No unused NavMap3D path query slot found! This should never happen :(.");
}
p_query_task.map_up = map_iteration.map_up;
@ -177,7 +179,7 @@ void NavMap::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task)
map_iteration.path_query_slots_semaphore.post();
}
Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
Vector3 NavMap3D::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
if (iteration_id == 0) {
NAVMAP_ITERATION_ZERO_ERROR_MSG();
return Vector3();
@ -188,7 +190,7 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector
return NavMeshQueries3D::map_iteration_get_closest_point_to_segment(map_iteration, p_from, p_to, p_use_collision);
}
Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
Vector3 NavMap3D::get_closest_point(const Vector3 &p_point) const {
if (iteration_id == 0) {
NAVMAP_ITERATION_ZERO_ERROR_MSG();
return Vector3();
@ -199,7 +201,7 @@ Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
return NavMeshQueries3D::map_iteration_get_closest_point(map_iteration, p_point);
}
Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
Vector3 NavMap3D::get_closest_point_normal(const Vector3 &p_point) const {
if (iteration_id == 0) {
NAVMAP_ITERATION_ZERO_ERROR_MSG();
return Vector3();
@ -210,7 +212,7 @@ Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
return NavMeshQueries3D::map_iteration_get_closest_point_normal(map_iteration, p_point);
}
RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
RID NavMap3D::get_closest_point_owner(const Vector3 &p_point) const {
if (iteration_id == 0) {
NAVMAP_ITERATION_ZERO_ERROR_MSG();
return RID();
@ -221,18 +223,18 @@ RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
return NavMeshQueries3D::map_iteration_get_closest_point_owner(map_iteration, p_point);
}
gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
ClosestPointQueryResult NavMap3D::get_closest_point_info(const Vector3 &p_point) const {
GET_MAP_ITERATION_CONST();
return NavMeshQueries3D::map_iteration_get_closest_point_info(map_iteration, p_point);
}
void NavMap::add_region(NavRegion *p_region) {
void NavMap3D::add_region(NavRegion3D *p_region) {
regions.push_back(p_region);
iteration_dirty = true;
}
void NavMap::remove_region(NavRegion *p_region) {
void NavMap3D::remove_region(NavRegion3D *p_region) {
int64_t region_index = regions.find(p_region);
if (region_index >= 0) {
regions.remove_at_unordered(region_index);
@ -240,12 +242,12 @@ void NavMap::remove_region(NavRegion *p_region) {
}
}
void NavMap::add_link(NavLink *p_link) {
void NavMap3D::add_link(NavLink3D *p_link) {
links.push_back(p_link);
iteration_dirty = true;
}
void NavMap::remove_link(NavLink *p_link) {
void NavMap3D::remove_link(NavLink3D *p_link) {
int64_t link_index = links.find(p_link);
if (link_index >= 0) {
links.remove_at_unordered(link_index);
@ -253,18 +255,18 @@ void NavMap::remove_link(NavLink *p_link) {
}
}
bool NavMap::has_agent(NavAgent *agent) const {
bool NavMap3D::has_agent(NavAgent3D *agent) const {
return agents.has(agent);
}
void NavMap::add_agent(NavAgent *agent) {
void NavMap3D::add_agent(NavAgent3D *agent) {
if (!has_agent(agent)) {
agents.push_back(agent);
agents_dirty = true;
}
}
void NavMap::remove_agent(NavAgent *agent) {
void NavMap3D::remove_agent(NavAgent3D *agent) {
remove_agent_as_controlled(agent);
int64_t agent_index = agents.find(agent);
if (agent_index >= 0) {
@ -273,11 +275,11 @@ void NavMap::remove_agent(NavAgent *agent) {
}
}
bool NavMap::has_obstacle(NavObstacle *obstacle) const {
bool NavMap3D::has_obstacle(NavObstacle3D *obstacle) const {
return obstacles.has(obstacle);
}
void NavMap::add_obstacle(NavObstacle *obstacle) {
void NavMap3D::add_obstacle(NavObstacle3D *obstacle) {
if (obstacle->get_paused()) {
// No point in adding a paused obstacle, it will add itself when unpaused again.
return;
@ -289,7 +291,7 @@ void NavMap::add_obstacle(NavObstacle *obstacle) {
}
}
void NavMap::remove_obstacle(NavObstacle *obstacle) {
void NavMap3D::remove_obstacle(NavObstacle3D *obstacle) {
int64_t obstacle_index = obstacles.find(obstacle);
if (obstacle_index >= 0) {
obstacles.remove_at_unordered(obstacle_index);
@ -297,7 +299,7 @@ void NavMap::remove_obstacle(NavObstacle *obstacle) {
}
}
void NavMap::set_agent_as_controlled(NavAgent *agent) {
void NavMap3D::set_agent_as_controlled(NavAgent3D *agent) {
remove_agent_as_controlled(agent);
if (agent->get_paused()) {
@ -320,7 +322,7 @@ void NavMap::set_agent_as_controlled(NavAgent *agent) {
}
}
void NavMap::remove_agent_as_controlled(NavAgent *agent) {
void NavMap3D::remove_agent_as_controlled(NavAgent3D *agent) {
int64_t agent_3d_index = active_3d_avoidance_agents.find(agent);
if (agent_3d_index >= 0) {
active_3d_avoidance_agents.remove_at_unordered(agent_3d_index);
@ -333,20 +335,20 @@ void NavMap::remove_agent_as_controlled(NavAgent *agent) {
}
}
Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
Vector3 NavMap3D::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
GET_MAP_ITERATION_CONST();
return NavMeshQueries3D::map_iteration_get_random_point(map_iteration, p_navigation_layers, p_uniformly);
}
void NavMap::_build_iteration() {
void NavMap3D::_build_iteration() {
if (!iteration_dirty || iteration_building || iteration_ready) {
return;
}
// Get the next free iteration slot that should be potentially unused.
iteration_slot_rwlock.read_lock();
NavMapIteration &next_map_iteration = iteration_slots[(iteration_slot_index + 1) % 2];
NavMapIteration3D &next_map_iteration = iteration_slots[(iteration_slot_index + 1) % 2];
// Check if the iteration slot is truly free or still used by an external thread.
bool iteration_is_free = next_map_iteration.users.get() == 0;
iteration_slot_rwlock.read_unlock();
@ -377,13 +379,13 @@ void NavMap::_build_iteration() {
uint32_t enabled_region_count = 0;
uint32_t enabled_link_count = 0;
for (NavRegion *region : regions) {
for (NavRegion3D *region : regions) {
if (!region->get_enabled()) {
continue;
}
enabled_region_count++;
}
for (NavLink *link : links) {
for (NavLink3D *link : links) {
if (!link->get_enabled()) {
continue;
}
@ -401,20 +403,20 @@ void NavMap::_build_iteration() {
uint32_t region_id_count = 0;
uint32_t link_id_count = 0;
for (NavRegion *region : regions) {
for (NavRegion3D *region : regions) {
if (!region->get_enabled()) {
continue;
}
NavRegionIteration &region_iteration = next_map_iteration.region_iterations[region_id_count];
NavRegionIteration3D &region_iteration = next_map_iteration.region_iterations[region_id_count];
region_iteration.id = region_id_count++;
region->get_iteration_update(region_iteration);
next_map_iteration.region_ptr_to_region_id[region] = (uint32_t)region_iteration.id;
}
for (NavLink *link : links) {
for (NavLink3D *link : links) {
if (!link->get_enabled()) {
continue;
}
NavLinkIteration &link_iteration = next_map_iteration.link_iterations[link_id_count];
NavLinkIteration3D &link_iteration = next_map_iteration.link_iterations[link_id_count];
link_iteration.id = link_id_count++;
link->get_iteration_update(link_iteration);
}
@ -424,7 +426,7 @@ void NavMap::_build_iteration() {
iteration_build.map_iteration = &next_map_iteration;
if (use_async_iterations) {
iteration_build_thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMap::_build_iteration_threaded, &iteration_build, true, SNAME("NavMapBuilder3D"));
iteration_build_thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMap3D::_build_iteration_threaded, &iteration_build, true, SNAME("NavMapBuilder3D"));
} else {
NavMapBuilder3D::build_navmap_iteration(iteration_build);
@ -433,13 +435,13 @@ void NavMap::_build_iteration() {
}
}
void NavMap::_build_iteration_threaded(void *p_arg) {
NavMapIterationBuild *_iteration_build = static_cast<NavMapIterationBuild *>(p_arg);
void NavMap3D::_build_iteration_threaded(void *p_arg) {
NavMapIterationBuild3D *_iteration_build = static_cast<NavMapIterationBuild3D *>(p_arg);
NavMapBuilder3D::build_navmap_iteration(*_iteration_build);
}
void NavMap::_sync_iteration() {
void NavMap3D::_sync_iteration() {
if (iteration_building || !iteration_ready) {
return;
}
@ -461,7 +463,7 @@ void NavMap::_sync_iteration() {
iteration_ready = false;
}
void NavMap::sync() {
void NavMap3D::sync() {
// Performance Monitor.
performance_data.pm_region_count = regions.size();
performance_data.pm_agent_count = agents.size();
@ -491,7 +493,7 @@ void NavMap::sync() {
_sync_avoidance();
}
void NavMap::_sync_avoidance() {
void NavMap3D::_sync_avoidance() {
_sync_dirty_avoidance_update_requests();
if (obstacles_dirty || agents_dirty) {
@ -502,9 +504,9 @@ void NavMap::_sync_avoidance() {
agents_dirty = false;
}
void NavMap::_update_rvo_obstacles_tree_2d() {
void NavMap3D::_update_rvo_obstacles_tree_2d() {
int obstacle_vertex_count = 0;
for (NavObstacle *obstacle : obstacles) {
for (NavObstacle3D *obstacle : obstacles) {
obstacle_vertex_count += obstacle->get_vertices().size();
}
@ -520,7 +522,7 @@ void NavMap::_update_rvo_obstacles_tree_2d() {
// The following block is modified copy from RVO2D::AddObstacle()
// Obstacles are linked and depend on all other obstacles.
for (NavObstacle *obstacle : obstacles) {
for (NavObstacle3D *obstacle : obstacles) {
const Vector3 &_obstacle_position = obstacle->get_position();
const Vector<Vector3> &_obstacle_vertices = obstacle->get_vertices();
@ -580,27 +582,27 @@ void NavMap::_update_rvo_obstacles_tree_2d() {
rvo_simulation_2d.kdTree_->buildObstacleTree(raw_obstacles);
}
void NavMap::_update_rvo_agents_tree_2d() {
void NavMap3D::_update_rvo_agents_tree_2d() {
// Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
std::vector<RVO2D::Agent2D *> raw_agents;
raw_agents.reserve(active_2d_avoidance_agents.size());
for (NavAgent *agent : active_2d_avoidance_agents) {
for (NavAgent3D *agent : active_2d_avoidance_agents) {
raw_agents.push_back(agent->get_rvo_agent_2d());
}
rvo_simulation_2d.kdTree_->buildAgentTree(raw_agents);
}
void NavMap::_update_rvo_agents_tree_3d() {
void NavMap3D::_update_rvo_agents_tree_3d() {
// Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
std::vector<RVO3D::Agent3D *> raw_agents;
raw_agents.reserve(active_3d_avoidance_agents.size());
for (NavAgent *agent : active_3d_avoidance_agents) {
for (NavAgent3D *agent : active_3d_avoidance_agents) {
raw_agents.push_back(agent->get_rvo_agent_3d());
}
rvo_simulation_3d.kdTree_->buildAgentTree(raw_agents);
}
void NavMap::_update_rvo_simulation() {
void NavMap3D::_update_rvo_simulation() {
if (obstacles_dirty) {
_update_rvo_obstacles_tree_2d();
}
@ -610,21 +612,21 @@ void NavMap::_update_rvo_simulation() {
}
}
void NavMap::compute_single_avoidance_step_2d(uint32_t index, NavAgent **agent) {
void NavMap3D::compute_single_avoidance_step_2d(uint32_t index, NavAgent3D **agent) {
(*(agent + index))->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
(*(agent + index))->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
(*(agent + index))->get_rvo_agent_2d()->update(&rvo_simulation_2d);
(*(agent + index))->update();
}
void NavMap::compute_single_avoidance_step_3d(uint32_t index, NavAgent **agent) {
void NavMap3D::compute_single_avoidance_step_3d(uint32_t index, NavAgent3D **agent) {
(*(agent + index))->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d);
(*(agent + index))->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d);
(*(agent + index))->get_rvo_agent_3d()->update(&rvo_simulation_3d);
(*(agent + index))->update();
}
void NavMap::step(real_t p_deltatime) {
void NavMap3D::step(real_t p_deltatime) {
deltatime = p_deltatime;
rvo_simulation_2d.setTimeStep(float(deltatime));
@ -632,10 +634,10 @@ void NavMap::step(real_t p_deltatime) {
if (active_2d_avoidance_agents.size() > 0) {
if (use_threads && avoidance_use_multiple_threads) {
WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_avoidance_step_2d, active_2d_avoidance_agents.ptr(), active_2d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents2D"));
WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap3D::compute_single_avoidance_step_2d, active_2d_avoidance_agents.ptr(), active_2d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents2D"));
WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
} else {
for (NavAgent *agent : active_2d_avoidance_agents) {
for (NavAgent3D *agent : active_2d_avoidance_agents) {
agent->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
agent->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
agent->get_rvo_agent_2d()->update(&rvo_simulation_2d);
@ -646,10 +648,10 @@ void NavMap::step(real_t p_deltatime) {
if (active_3d_avoidance_agents.size() > 0) {
if (use_threads && avoidance_use_multiple_threads) {
WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_avoidance_step_3d, active_3d_avoidance_agents.ptr(), active_3d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents3D"));
WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap3D::compute_single_avoidance_step_3d, active_3d_avoidance_agents.ptr(), active_3d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents3D"));
WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
} else {
for (NavAgent *agent : active_3d_avoidance_agents) {
for (NavAgent3D *agent : active_3d_avoidance_agents) {
agent->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d);
agent->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d);
agent->get_rvo_agent_3d()->update(&rvo_simulation_3d);
@ -659,30 +661,30 @@ void NavMap::step(real_t p_deltatime) {
}
}
void NavMap::dispatch_callbacks() {
for (NavAgent *agent : active_2d_avoidance_agents) {
void NavMap3D::dispatch_callbacks() {
for (NavAgent3D *agent : active_2d_avoidance_agents) {
agent->dispatch_avoidance_callback();
}
for (NavAgent *agent : active_3d_avoidance_agents) {
for (NavAgent3D *agent : active_3d_avoidance_agents) {
agent->dispatch_avoidance_callback();
}
}
void NavMap::_update_merge_rasterizer_cell_dimensions() {
void NavMap3D::_update_merge_rasterizer_cell_dimensions() {
merge_rasterizer_cell_size.x = cell_size * merge_rasterizer_cell_scale;
merge_rasterizer_cell_size.y = cell_height * merge_rasterizer_cell_scale;
merge_rasterizer_cell_size.z = cell_size * merge_rasterizer_cell_scale;
}
int NavMap::get_region_connections_count(NavRegion *p_region) const {
int NavMap3D::get_region_connections_count(NavRegion3D *p_region) const {
ERR_FAIL_NULL_V(p_region, 0);
GET_MAP_ITERATION_CONST();
HashMap<NavRegion *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
HashMap<NavRegion3D *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
if (found_id) {
HashMap<uint32_t, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
HashMap<uint32_t, LocalVector<Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
if (found_connections) {
return found_connections->value.size();
}
@ -691,14 +693,14 @@ int NavMap::get_region_connections_count(NavRegion *p_region) const {
return 0;
}
Vector3 NavMap::get_region_connection_pathway_start(NavRegion *p_region, int p_connection_id) const {
Vector3 NavMap3D::get_region_connection_pathway_start(NavRegion3D *p_region, int p_connection_id) const {
ERR_FAIL_NULL_V(p_region, Vector3());
GET_MAP_ITERATION_CONST();
HashMap<NavRegion *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
HashMap<NavRegion3D *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
if (found_id) {
HashMap<uint32_t, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
HashMap<uint32_t, LocalVector<Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
if (found_connections) {
ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3());
return found_connections->value[p_connection_id].pathway_start;
@ -708,14 +710,14 @@ Vector3 NavMap::get_region_connection_pathway_start(NavRegion *p_region, int p_c
return Vector3();
}
Vector3 NavMap::get_region_connection_pathway_end(NavRegion *p_region, int p_connection_id) const {
Vector3 NavMap3D::get_region_connection_pathway_end(NavRegion3D *p_region, int p_connection_id) const {
ERR_FAIL_NULL_V(p_region, Vector3());
GET_MAP_ITERATION_CONST();
HashMap<NavRegion *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
HashMap<NavRegion3D *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
if (found_id) {
HashMap<uint32_t, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
HashMap<uint32_t, LocalVector<Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
if (found_connections) {
ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3());
return found_connections->value[p_connection_id].pathway_end;
@ -725,66 +727,66 @@ Vector3 NavMap::get_region_connection_pathway_end(NavRegion *p_region, int p_con
return Vector3();
}
void NavMap::add_region_sync_dirty_request(SelfList<NavRegion> *p_sync_request) {
void NavMap3D::add_region_sync_dirty_request(SelfList<NavRegion3D> *p_sync_request) {
if (p_sync_request->in_list()) {
return;
}
sync_dirty_requests.regions.add(p_sync_request);
}
void NavMap::add_link_sync_dirty_request(SelfList<NavLink> *p_sync_request) {
void NavMap3D::add_link_sync_dirty_request(SelfList<NavLink3D> *p_sync_request) {
if (p_sync_request->in_list()) {
return;
}
sync_dirty_requests.links.add(p_sync_request);
}
void NavMap::add_agent_sync_dirty_request(SelfList<NavAgent> *p_sync_request) {
void NavMap3D::add_agent_sync_dirty_request(SelfList<NavAgent3D> *p_sync_request) {
if (p_sync_request->in_list()) {
return;
}
sync_dirty_requests.agents.add(p_sync_request);
}
void NavMap::add_obstacle_sync_dirty_request(SelfList<NavObstacle> *p_sync_request) {
void NavMap3D::add_obstacle_sync_dirty_request(SelfList<NavObstacle3D> *p_sync_request) {
if (p_sync_request->in_list()) {
return;
}
sync_dirty_requests.obstacles.add(p_sync_request);
}
void NavMap::remove_region_sync_dirty_request(SelfList<NavRegion> *p_sync_request) {
void NavMap3D::remove_region_sync_dirty_request(SelfList<NavRegion3D> *p_sync_request) {
if (!p_sync_request->in_list()) {
return;
}
sync_dirty_requests.regions.remove(p_sync_request);
}
void NavMap::remove_link_sync_dirty_request(SelfList<NavLink> *p_sync_request) {
void NavMap3D::remove_link_sync_dirty_request(SelfList<NavLink3D> *p_sync_request) {
if (!p_sync_request->in_list()) {
return;
}
sync_dirty_requests.links.remove(p_sync_request);
}
void NavMap::remove_agent_sync_dirty_request(SelfList<NavAgent> *p_sync_request) {
void NavMap3D::remove_agent_sync_dirty_request(SelfList<NavAgent3D> *p_sync_request) {
if (!p_sync_request->in_list()) {
return;
}
sync_dirty_requests.agents.remove(p_sync_request);
}
void NavMap::remove_obstacle_sync_dirty_request(SelfList<NavObstacle> *p_sync_request) {
void NavMap3D::remove_obstacle_sync_dirty_request(SelfList<NavObstacle3D> *p_sync_request) {
if (!p_sync_request->in_list()) {
return;
}
sync_dirty_requests.obstacles.remove(p_sync_request);
}
void NavMap::_sync_dirty_map_update_requests() {
void NavMap3D::_sync_dirty_map_update_requests() {
// If entire map settings changed make all regions dirty.
if (map_settings_dirty) {
for (NavRegion *region : regions) {
for (NavRegion3D *region : regions) {
region->scratch_polygons();
}
iteration_dirty = true;
@ -795,24 +797,24 @@ void NavMap::_sync_dirty_map_update_requests() {
}
// Sync NavRegions.
for (SelfList<NavRegion> *element = sync_dirty_requests.regions.first(); element; element = element->next()) {
for (SelfList<NavRegion3D> *element = sync_dirty_requests.regions.first(); element; element = element->next()) {
element->self()->sync();
}
sync_dirty_requests.regions.clear();
// Sync NavLinks.
for (SelfList<NavLink> *element = sync_dirty_requests.links.first(); element; element = element->next()) {
for (SelfList<NavLink3D> *element = sync_dirty_requests.links.first(); element; element = element->next()) {
element->self()->sync();
}
sync_dirty_requests.links.clear();
}
void NavMap::_sync_dirty_avoidance_update_requests() {
void NavMap3D::_sync_dirty_avoidance_update_requests() {
// Sync NavAgents.
if (!agents_dirty) {
agents_dirty = sync_dirty_requests.agents.first();
}
for (SelfList<NavAgent> *element = sync_dirty_requests.agents.first(); element; element = element->next()) {
for (SelfList<NavAgent3D> *element = sync_dirty_requests.agents.first(); element; element = element->next()) {
element->self()->sync();
}
sync_dirty_requests.agents.clear();
@ -821,13 +823,13 @@ void NavMap::_sync_dirty_avoidance_update_requests() {
if (!obstacles_dirty) {
obstacles_dirty = sync_dirty_requests.obstacles.first();
}
for (SelfList<NavObstacle> *element = sync_dirty_requests.obstacles.first(); element; element = element->next()) {
for (SelfList<NavObstacle3D> *element = sync_dirty_requests.obstacles.first(); element; element = element->next()) {
element->self()->sync();
}
sync_dirty_requests.obstacles.clear();
}
void NavMap::set_use_async_iterations(bool p_enabled) {
void NavMap3D::set_use_async_iterations(bool p_enabled) {
if (use_async_iterations == p_enabled) {
return;
}
@ -836,11 +838,11 @@ void NavMap::set_use_async_iterations(bool p_enabled) {
#endif
}
bool NavMap::get_use_async_iterations() const {
bool NavMap3D::get_use_async_iterations() const {
return use_async_iterations;
}
NavMap::NavMap() {
NavMap3D::NavMap3D() {
avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads");
avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads");
@ -859,7 +861,7 @@ NavMap::NavMap() {
iteration_slots.resize(2);
for (NavMapIteration &iteration_slot : iteration_slots) {
for (NavMapIteration3D &iteration_slot : iteration_slots) {
iteration_slot.path_query_slots.resize(path_query_slots_max);
for (uint32_t i = 0; i < iteration_slot.path_query_slots.size(); i++) {
iteration_slot.path_query_slots[i].slot_index = i;
@ -874,7 +876,7 @@ NavMap::NavMap() {
#endif
}
NavMap::~NavMap() {
NavMap3D::~NavMap3D() {
if (iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) {
WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id);
iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;

110
modules/navigation/nav_map.h → modules/navigation/nav_map_3d.h
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_map.h */
/* nav_map_3d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -32,8 +32,8 @@
#include "3d/nav_map_iteration_3d.h"
#include "3d/nav_mesh_queries_3d.h"
#include "nav_rid.h"
#include "nav_utils.h"
#include "nav_rid_3d.h"
#include "nav_utils_3d.h"
#include "core/math/math_defs.h"
#include "core/object/worker_thread_pool.h"
@ -44,12 +44,12 @@
#include <RVOSimulator2d.h>
#include <RVOSimulator3d.h>
class NavLink;
class NavRegion;
class NavAgent;
class NavObstacle;
class NavLink3D;
class NavRegion3D;
class NavAgent3D;
class NavObstacle3D;
class NavMap : public NavRid {
class NavMap3D : public NavRid3D {
/// Map Up
Vector3 up = Vector3(0, 1, 0);
@ -74,27 +74,27 @@ class NavMap : public NavRid {
bool map_settings_dirty = true;
/// Map regions
LocalVector<NavRegion *> regions;
LocalVector<NavRegion3D *> regions;
/// Map links
LocalVector<NavLink *> links;
LocalVector<NavLink3D *> links;
/// RVO avoidance worlds
RVO2D::RVOSimulator2D rvo_simulation_2d;
RVO3D::RVOSimulator3D rvo_simulation_3d;
/// avoidance controlled agents
LocalVector<NavAgent *> active_2d_avoidance_agents;
LocalVector<NavAgent *> active_3d_avoidance_agents;
LocalVector<NavAgent3D *> active_2d_avoidance_agents;
LocalVector<NavAgent3D *> active_3d_avoidance_agents;
/// dirty flag when one of the agent's arrays are modified
bool agents_dirty = true;
/// All the Agents (even the controlled one)
LocalVector<NavAgent *> agents;
LocalVector<NavAgent3D *> agents;
/// All the avoidance obstacles (both static and dynamic)
LocalVector<NavObstacle *> obstacles;
LocalVector<NavObstacle3D *> obstacles;
/// Are rvo obstacles modified?
bool obstacles_dirty = true;
@ -110,13 +110,13 @@ class NavMap : public NavRid {
bool avoidance_use_high_priority_threads = true;
// Performance Monitor
gd::PerformanceData performance_data;
nav_3d::PerformanceData performance_data;
struct {
SelfList<NavRegion>::List regions;
SelfList<NavLink>::List links;
SelfList<NavAgent>::List agents;
SelfList<NavObstacle>::List obstacles;
SelfList<NavRegion3D>::List regions;
SelfList<NavLink3D>::List links;
SelfList<NavAgent3D>::List agents;
SelfList<NavObstacle3D>::List obstacles;
} sync_dirty_requests;
int path_query_slots_max = 4;
@ -124,10 +124,10 @@ class NavMap : public NavRid {
bool use_async_iterations = true;
uint32_t iteration_slot_index = 0;
LocalVector<NavMapIteration> iteration_slots;
LocalVector<NavMapIteration3D> iteration_slots;
mutable RWLock iteration_slot_rwlock;
NavMapIterationBuild iteration_build;
NavMapIterationBuild3D iteration_build;
bool iteration_build_use_threads = false;
WorkerThreadPool::TaskID iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
static void _build_iteration_threaded(void *p_arg);
@ -140,8 +140,8 @@ class NavMap : public NavRid {
void _sync_iteration();
public:
NavMap();
~NavMap();
NavMap3D();
~NavMap3D();
uint32_t get_iteration_id() const { return iteration_id; }
@ -178,7 +178,7 @@ public:
return link_connection_radius;
}
gd::PointKey get_point_key(const Vector3 &p_pos) const;
nav_3d::PointKey get_point_key(const Vector3 &p_pos) const;
const Vector3 &get_merge_rasterizer_cell_size() const;
void query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task);
@ -186,35 +186,35 @@ public:
Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
Vector3 get_closest_point(const Vector3 &p_point) const;
Vector3 get_closest_point_normal(const Vector3 &p_point) const;
gd::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
nav_3d::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
RID get_closest_point_owner(const Vector3 &p_point) const;
void add_region(NavRegion *p_region);
void remove_region(NavRegion *p_region);
const LocalVector<NavRegion *> &get_regions() const {
void add_region(NavRegion3D *p_region);
void remove_region(NavRegion3D *p_region);
const LocalVector<NavRegion3D *> &get_regions() const {
return regions;
}
void add_link(NavLink *p_link);
void remove_link(NavLink *p_link);
const LocalVector<NavLink *> &get_links() const {
void add_link(NavLink3D *p_link);
void remove_link(NavLink3D *p_link);
const LocalVector<NavLink3D *> &get_links() const {
return links;
}
bool has_agent(NavAgent *agent) const;
void add_agent(NavAgent *agent);
void remove_agent(NavAgent *agent);
const LocalVector<NavAgent *> &get_agents() const {
bool has_agent(NavAgent3D *agent) const;
void add_agent(NavAgent3D *agent);
void remove_agent(NavAgent3D *agent);
const LocalVector<NavAgent3D *> &get_agents() const {
return agents;
}
void set_agent_as_controlled(NavAgent *agent);
void remove_agent_as_controlled(NavAgent *agent);
void set_agent_as_controlled(NavAgent3D *agent);
void remove_agent_as_controlled(NavAgent3D *agent);
bool has_obstacle(NavObstacle *obstacle) const;
void add_obstacle(NavObstacle *obstacle);
void remove_obstacle(NavObstacle *obstacle);
const LocalVector<NavObstacle *> &get_obstacles() const {
bool has_obstacle(NavObstacle3D *obstacle) const;
void add_obstacle(NavObstacle3D *obstacle);
void remove_obstacle(NavObstacle3D *obstacle);
const LocalVector<NavObstacle3D *> &get_obstacles() const {
return obstacles;
}
@ -235,19 +235,19 @@ public:
int get_pm_edge_free_count() const { return performance_data.pm_edge_free_count; }
int get_pm_obstacle_count() const { return performance_data.pm_obstacle_count; }
int get_region_connections_count(NavRegion *p_region) const;
Vector3 get_region_connection_pathway_start(NavRegion *p_region, int p_connection_id) const;
Vector3 get_region_connection_pathway_end(NavRegion *p_region, int p_connection_id) const;
int get_region_connections_count(NavRegion3D *p_region) const;
Vector3 get_region_connection_pathway_start(NavRegion3D *p_region, int p_connection_id) const;
Vector3 get_region_connection_pathway_end(NavRegion3D *p_region, int p_connection_id) const;
void add_region_sync_dirty_request(SelfList<NavRegion> *p_sync_request);
void add_link_sync_dirty_request(SelfList<NavLink> *p_sync_request);
void add_agent_sync_dirty_request(SelfList<NavAgent> *p_sync_request);
void add_obstacle_sync_dirty_request(SelfList<NavObstacle> *p_sync_request);
void add_region_sync_dirty_request(SelfList<NavRegion3D> *p_sync_request);
void add_link_sync_dirty_request(SelfList<NavLink3D> *p_sync_request);
void add_agent_sync_dirty_request(SelfList<NavAgent3D> *p_sync_request);
void add_obstacle_sync_dirty_request(SelfList<NavObstacle3D> *p_sync_request);
void remove_region_sync_dirty_request(SelfList<NavRegion> *p_sync_request);
void remove_link_sync_dirty_request(SelfList<NavLink> *p_sync_request);
void remove_agent_sync_dirty_request(SelfList<NavAgent> *p_sync_request);
void remove_obstacle_sync_dirty_request(SelfList<NavObstacle> *p_sync_request);
void remove_region_sync_dirty_request(SelfList<NavRegion3D> *p_sync_request);
void remove_link_sync_dirty_request(SelfList<NavLink3D> *p_sync_request);
void remove_agent_sync_dirty_request(SelfList<NavAgent3D> *p_sync_request);
void remove_obstacle_sync_dirty_request(SelfList<NavObstacle3D> *p_sync_request);
void set_use_async_iterations(bool p_enabled);
bool get_use_async_iterations() const;
@ -256,10 +256,10 @@ private:
void _sync_dirty_map_update_requests();
void _sync_dirty_avoidance_update_requests();
void compute_single_step(uint32_t index, NavAgent **agent);
void compute_single_step(uint32_t index, NavAgent3D **agent);
void compute_single_avoidance_step_2d(uint32_t index, NavAgent **agent);
void compute_single_avoidance_step_3d(uint32_t index, NavAgent **agent);
void compute_single_avoidance_step_2d(uint32_t index, NavAgent3D **agent);
void compute_single_avoidance_step_3d(uint32_t index, NavAgent3D **agent);
void _sync_avoidance();
void _update_rvo_simulation();

48
modules/navigation/nav_obstacle.cpp → modules/navigation/nav_obstacle_3d.cpp
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_obstacle.cpp */
/* nav_obstacle_3d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -28,12 +28,12 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "nav_obstacle.h"
#include "nav_obstacle_3d.h"
#include "nav_agent.h"
#include "nav_map.h"
#include "nav_agent_3d.h"
#include "nav_map_3d.h"
void NavObstacle::set_agent(NavAgent *p_agent) {
void NavObstacle3D::set_agent(NavAgent3D *p_agent) {
if (agent == p_agent) {
return;
}
@ -45,7 +45,7 @@ void NavObstacle::set_agent(NavAgent *p_agent) {
request_sync();
}
void NavObstacle::set_avoidance_enabled(bool p_enabled) {
void NavObstacle3D::set_avoidance_enabled(bool p_enabled) {
if (avoidance_enabled == p_enabled) {
return;
}
@ -58,7 +58,7 @@ void NavObstacle::set_avoidance_enabled(bool p_enabled) {
request_sync();
}
void NavObstacle::set_use_3d_avoidance(bool p_enabled) {
void NavObstacle3D::set_use_3d_avoidance(bool p_enabled) {
if (use_3d_avoidance == p_enabled) {
return;
}
@ -73,7 +73,7 @@ void NavObstacle::set_use_3d_avoidance(bool p_enabled) {
request_sync();
}
void NavObstacle::set_map(NavMap *p_map) {
void NavObstacle3D::set_map(NavMap3D *p_map) {
if (map == p_map) {
return;
}
@ -98,7 +98,7 @@ void NavObstacle::set_map(NavMap *p_map) {
}
}
void NavObstacle::set_position(const Vector3 p_position) {
void NavObstacle3D::set_position(const Vector3 p_position) {
if (position == p_position) {
return;
}
@ -113,7 +113,7 @@ void NavObstacle::set_position(const Vector3 p_position) {
request_sync();
}
void NavObstacle::set_radius(real_t p_radius) {
void NavObstacle3D::set_radius(real_t p_radius) {
if (radius == p_radius) {
return;
}
@ -125,7 +125,7 @@ void NavObstacle::set_radius(real_t p_radius) {
}
}
void NavObstacle::set_height(const real_t p_height) {
void NavObstacle3D::set_height(const real_t p_height) {
if (height == p_height) {
return;
}
@ -140,7 +140,7 @@ void NavObstacle::set_height(const real_t p_height) {
request_sync();
}
void NavObstacle::set_velocity(const Vector3 p_velocity) {
void NavObstacle3D::set_velocity(const Vector3 p_velocity) {
velocity = p_velocity;
if (agent) {
@ -148,7 +148,7 @@ void NavObstacle::set_velocity(const Vector3 p_velocity) {
}
}
void NavObstacle::set_vertices(const Vector<Vector3> &p_vertices) {
void NavObstacle3D::set_vertices(const Vector<Vector3> &p_vertices) {
if (vertices == p_vertices) {
return;
}
@ -159,7 +159,7 @@ void NavObstacle::set_vertices(const Vector<Vector3> &p_vertices) {
request_sync();
}
bool NavObstacle::is_map_changed() {
bool NavObstacle3D::is_map_changed() {
if (map) {
bool is_changed = map->get_iteration_id() != last_map_iteration_id;
last_map_iteration_id = map->get_iteration_id();
@ -169,7 +169,7 @@ bool NavObstacle::is_map_changed() {
}
}
void NavObstacle::set_avoidance_layers(uint32_t p_layers) {
void NavObstacle3D::set_avoidance_layers(uint32_t p_layers) {
if (avoidance_layers == p_layers) {
return;
}
@ -184,15 +184,15 @@ void NavObstacle::set_avoidance_layers(uint32_t p_layers) {
request_sync();
}
bool NavObstacle::is_dirty() const {
bool NavObstacle3D::is_dirty() const {
return obstacle_dirty;
}
void NavObstacle::sync() {
void NavObstacle3D::sync() {
obstacle_dirty = false;
}
void NavObstacle::internal_update_agent() {
void NavObstacle3D::internal_update_agent() {
if (agent) {
agent->set_neighbor_distance(0.0);
agent->set_max_neighbors(0.0);
@ -212,7 +212,7 @@ void NavObstacle::internal_update_agent() {
}
}
void NavObstacle::set_paused(bool p_paused) {
void NavObstacle3D::set_paused(bool p_paused) {
if (paused == p_paused) {
return;
}
@ -229,26 +229,26 @@ void NavObstacle::set_paused(bool p_paused) {
internal_update_agent();
}
bool NavObstacle::get_paused() const {
bool NavObstacle3D::get_paused() const {
return paused;
}
void NavObstacle::request_sync() {
void NavObstacle3D::request_sync() {
if (map && !sync_dirty_request_list_element.in_list()) {
map->add_obstacle_sync_dirty_request(&sync_dirty_request_list_element);
}
}
void NavObstacle::cancel_sync_request() {
void NavObstacle3D::cancel_sync_request() {
if (map && sync_dirty_request_list_element.in_list()) {
map->remove_obstacle_sync_dirty_request(&sync_dirty_request_list_element);
}
}
NavObstacle::NavObstacle() :
NavObstacle3D::NavObstacle3D() :
sync_dirty_request_list_element(this) {
}
NavObstacle::~NavObstacle() {
NavObstacle3D::~NavObstacle3D() {
cancel_sync_request();
}

28
modules/navigation/nav_obstacle.h → modules/navigation/nav_obstacle_3d.h
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_obstacle.h */
/* nav_obstacle_3d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -30,17 +30,17 @@
#pragma once
#include "nav_rid.h"
#include "nav_rid_3d.h"
#include "core/object/class_db.h"
#include "core/templates/self_list.h"
class NavAgent;
class NavMap;
class NavAgent3D;
class NavMap3D;
class NavObstacle : public NavRid {
NavAgent *agent = nullptr;
NavMap *map = nullptr;
class NavObstacle3D : public NavRid3D {
NavAgent3D *agent = nullptr;
NavMap3D *map = nullptr;
Vector3 velocity;
Vector3 position;
Vector<Vector3> vertices;
@ -57,11 +57,11 @@ class NavObstacle : public NavRid {
uint32_t last_map_iteration_id = 0;
bool paused = false;
SelfList<NavObstacle> sync_dirty_request_list_element;
SelfList<NavObstacle3D> sync_dirty_request_list_element;
public:
NavObstacle();
~NavObstacle();
NavObstacle3D();
~NavObstacle3D();
void set_avoidance_enabled(bool p_enabled);
bool is_avoidance_enabled() { return avoidance_enabled; }
@ -69,11 +69,11 @@ public:
void set_use_3d_avoidance(bool p_enabled);
bool get_use_3d_avoidance() { return use_3d_avoidance; }
void set_map(NavMap *p_map);
NavMap *get_map() { return map; }
void set_map(NavMap3D *p_map);
NavMap3D *get_map() { return map; }
void set_agent(NavAgent *p_agent);
NavAgent *get_agent() { return agent; }
void set_agent(NavAgent3D *p_agent);
NavAgent3D *get_agent() { return agent; }
void set_position(const Vector3 p_position);
const Vector3 &get_position() const { return position; }

50
modules/navigation/nav_region.cpp → modules/navigation/nav_region_3d.cpp
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_region.cpp */
/* nav_region_3d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -28,15 +28,17 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#include "nav_region.h"
#include "nav_region_3d.h"
#include "nav_map.h"
#include "nav_map_3d.h"
#include "3d/nav_map_builder_3d.h"
#include "3d/nav_mesh_queries_3d.h"
#include "3d/nav_region_iteration_3d.h"
void NavRegion::set_map(NavMap *p_map) {
using namespace nav_3d;
void NavRegion3D::set_map(NavMap3D *p_map) {
if (map == p_map) {
return;
}
@ -56,7 +58,7 @@ void NavRegion::set_map(NavMap *p_map) {
}
}
void NavRegion::set_enabled(bool p_enabled) {
void NavRegion3D::set_enabled(bool p_enabled) {
if (enabled == p_enabled) {
return;
}
@ -68,7 +70,7 @@ void NavRegion::set_enabled(bool p_enabled) {
request_sync();
}
void NavRegion::set_use_edge_connections(bool p_enabled) {
void NavRegion3D::set_use_edge_connections(bool p_enabled) {
if (use_edge_connections != p_enabled) {
use_edge_connections = p_enabled;
polygons_dirty = true;
@ -77,7 +79,7 @@ void NavRegion::set_use_edge_connections(bool p_enabled) {
request_sync();
}
void NavRegion::set_transform(Transform3D p_transform) {
void NavRegion3D::set_transform(Transform3D p_transform) {
if (transform == p_transform) {
return;
}
@ -93,7 +95,7 @@ void NavRegion::set_transform(Transform3D p_transform) {
#endif // DEBUG_ENABLED
}
void NavRegion::set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) {
void NavRegion3D::set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) {
#ifdef DEBUG_ENABLED
if (map && p_navigation_mesh.is_valid() && !Math::is_equal_approx(double(map->get_cell_size()), double(p_navigation_mesh->get_cell_size()))) {
ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_size` of %s while assigned to a navigation map set to a `cell_size` of %s. The cell size for navigation maps can be changed by using the NavigationServer map_set_cell_size() function. The cell size for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_mesh->get_cell_size()), double(map->get_cell_size())));
@ -118,20 +120,20 @@ void NavRegion::set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) {
request_sync();
}
Vector3 NavRegion::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const {
Vector3 NavRegion3D::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const {
RWLockRead read_lock(region_rwlock);
return NavMeshQueries3D::polygons_get_closest_point_to_segment(
get_polygons(), p_from, p_to, p_use_collision);
}
gd::ClosestPointQueryResult NavRegion::get_closest_point_info(const Vector3 &p_point) const {
ClosestPointQueryResult NavRegion3D::get_closest_point_info(const Vector3 &p_point) const {
RWLockRead read_lock(region_rwlock);
return NavMeshQueries3D::polygons_get_closest_point_info(get_polygons(), p_point);
}
Vector3 NavRegion::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
Vector3 NavRegion3D::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
RWLockRead read_lock(region_rwlock);
if (!get_enabled()) {
@ -141,7 +143,7 @@ Vector3 NavRegion::get_random_point(uint32_t p_navigation_layers, bool p_uniform
return NavMeshQueries3D::polygons_get_random_point(get_polygons(), p_navigation_layers, p_uniformly);
}
void NavRegion::set_navigation_layers(uint32_t p_navigation_layers) {
void NavRegion3D::set_navigation_layers(uint32_t p_navigation_layers) {
if (navigation_layers == p_navigation_layers) {
return;
}
@ -151,7 +153,7 @@ void NavRegion::set_navigation_layers(uint32_t p_navigation_layers) {
request_sync();
}
void NavRegion::set_enter_cost(real_t p_enter_cost) {
void NavRegion3D::set_enter_cost(real_t p_enter_cost) {
real_t new_enter_cost = MAX(p_enter_cost, 0.0);
if (enter_cost == new_enter_cost) {
return;
@ -162,7 +164,7 @@ void NavRegion::set_enter_cost(real_t p_enter_cost) {
request_sync();
}
void NavRegion::set_travel_cost(real_t p_travel_cost) {
void NavRegion3D::set_travel_cost(real_t p_travel_cost) {
real_t new_travel_cost = MAX(p_travel_cost, 0.0);
if (travel_cost == new_travel_cost) {
return;
@ -173,7 +175,7 @@ void NavRegion::set_travel_cost(real_t p_travel_cost) {
request_sync();
}
void NavRegion::set_owner_id(ObjectID p_owner_id) {
void NavRegion3D::set_owner_id(ObjectID p_owner_id) {
if (owner_id == p_owner_id) {
return;
}
@ -183,7 +185,7 @@ void NavRegion::set_owner_id(ObjectID p_owner_id) {
request_sync();
}
bool NavRegion::sync() {
bool NavRegion3D::sync() {
RWLockWrite write_lock(region_rwlock);
bool something_changed = region_dirty || polygons_dirty;
@ -195,7 +197,7 @@ bool NavRegion::sync() {
return something_changed;
}
void NavRegion::update_polygons() {
void NavRegion3D::update_polygons() {
if (!polygons_dirty) {
return;
}
@ -229,7 +231,7 @@ void NavRegion::update_polygons() {
bool first_vertex = true;
int navigation_mesh_polygon_index = 0;
for (gd::Polygon &polygon : navmesh_polygons) {
for (Polygon &polygon : navmesh_polygons) {
polygon.surface_area = 0.0;
Vector<int> navigation_mesh_polygon = pending_navmesh_polygons[navigation_mesh_polygon_index];
@ -288,7 +290,7 @@ void NavRegion::update_polygons() {
bounds = _new_bounds;
}
void NavRegion::get_iteration_update(NavRegionIteration &r_iteration) {
void NavRegion3D::get_iteration_update(NavRegionIteration3D &r_iteration) {
r_iteration.navigation_layers = get_navigation_layers();
r_iteration.enter_cost = get_enter_cost();
r_iteration.travel_cost = get_travel_cost();
@ -305,29 +307,29 @@ void NavRegion::get_iteration_update(NavRegionIteration &r_iteration) {
r_iteration.navmesh_polygons.clear();
r_iteration.navmesh_polygons.resize(navmesh_polygons.size());
for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
gd::Polygon &navmesh_polygon = navmesh_polygons[i];
Polygon &navmesh_polygon = navmesh_polygons[i];
navmesh_polygon.owner = &r_iteration;
r_iteration.navmesh_polygons[i] = navmesh_polygon;
}
}
void NavRegion::request_sync() {
void NavRegion3D::request_sync() {
if (map && !sync_dirty_request_list_element.in_list()) {
map->add_region_sync_dirty_request(&sync_dirty_request_list_element);
}
}
void NavRegion::cancel_sync_request() {
void NavRegion3D::cancel_sync_request() {
if (map && sync_dirty_request_list_element.in_list()) {
map->remove_region_sync_dirty_request(&sync_dirty_request_list_element);
}
}
NavRegion::NavRegion() :
NavRegion3D::NavRegion3D() :
sync_dirty_request_list_element(this) {
type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION;
}
NavRegion::~NavRegion() {
NavRegion3D::~NavRegion3D() {
cancel_sync_request();
}

30
modules/navigation/nav_region.h → modules/navigation/nav_region_3d.h
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_region.h */
/* nav_region_3d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -30,18 +30,18 @@
#pragma once
#include "nav_base.h"
#include "nav_utils.h"
#include "nav_base_3d.h"
#include "nav_utils_3d.h"
#include "core/os/rw_lock.h"
#include "scene/resources/navigation_mesh.h"
struct NavRegionIteration;
struct NavRegionIteration3D;
class NavRegion : public NavBase {
class NavRegion3D : public NavBase3D {
RWLock region_rwlock;
NavMap *map = nullptr;
NavMap3D *map = nullptr;
Transform3D transform;
bool enabled = true;
@ -50,7 +50,7 @@ class NavRegion : public NavBase {
bool region_dirty = true;
bool polygons_dirty = true;
LocalVector<gd::Polygon> navmesh_polygons;
LocalVector<nav_3d::Polygon> navmesh_polygons;
real_t surface_area = 0.0;
AABB bounds;
@ -59,11 +59,11 @@ class NavRegion : public NavBase {
Vector<Vector3> pending_navmesh_vertices;
Vector<Vector<int>> pending_navmesh_polygons;
SelfList<NavRegion> sync_dirty_request_list_element;
SelfList<NavRegion3D> sync_dirty_request_list_element;
public:
NavRegion();
~NavRegion();
NavRegion3D();
~NavRegion3D();
void scratch_polygons() {
polygons_dirty = true;
@ -72,8 +72,8 @@ public:
void set_enabled(bool p_enabled);
bool get_enabled() const { return enabled; }
void set_map(NavMap *p_map);
NavMap *get_map() const {
void set_map(NavMap3D *p_map);
NavMap3D *get_map() const {
return map;
}
@ -87,12 +87,12 @@ public:
void set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh);
LocalVector<gd::Polygon> const &get_polygons() const {
LocalVector<nav_3d::Polygon> const &get_polygons() const {
return navmesh_polygons;
}
Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const;
gd::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
nav_3d::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
Vector3 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const;
real_t get_surface_area() const { return surface_area; }
@ -108,7 +108,7 @@ public:
void request_sync();
void cancel_sync_request();
void get_iteration_update(NavRegionIteration &r_iteration);
void get_iteration_update(NavRegionIteration3D &r_iteration);
private:
void update_polygons();

4
modules/navigation/nav_rid.h → modules/navigation/nav_rid_3d.h
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_rid.h */
/* nav_rid_3d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -32,7 +32,7 @@
#include "core/templates/rid.h"
class NavRid {
class NavRid3D {
RID self;
public:

139
modules/navigation/nav_utils.h → modules/navigation/nav_utils_3d.h
View File

@ -1,5 +1,5 @@
/**************************************************************************/
/* nav_utils.h */
/* nav_utils_3d.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@ -33,12 +33,12 @@
#include "core/math/vector3.h"
#include "core/templates/hash_map.h"
#include "core/templates/hashfuncs.h"
#include "core/templates/local_vector.h"
#include "servers/navigation/nav_heap.h"
#include "servers/navigation/navigation_utilities.h"
struct NavBaseIteration;
struct NavBaseIteration3D;
namespace gd {
namespace nav_3d {
struct Polygon;
union PointKey {
@ -102,7 +102,7 @@ struct Polygon {
uint32_t id = UINT32_MAX;
/// Navigation region or link that contains this polygon.
const NavBaseIteration *owner = nullptr;
const NavBaseIteration3D *owner = nullptr;
/// The points of this `Polygon`
LocalVector<Point> points;
@ -184,133 +184,8 @@ struct ClosestPointQueryResult {
RID owner;
};
template <typename T>
struct NoopIndexer {
void operator()(const T &p_value, uint32_t p_index) {}
};
/**
* A max-heap implementation that notifies of element index changes.
*/
template <typename T, typename LessThan = Comparator<T>, typename Indexer = NoopIndexer<T>>
class Heap {
LocalVector<T> _buffer;
LessThan _less_than;
Indexer _indexer;
public:
static constexpr uint32_t INVALID_INDEX = UINT32_MAX;
void reserve(uint32_t p_size) {
_buffer.reserve(p_size);
}
uint32_t size() const {
return _buffer.size();
}
bool is_empty() const {
return _buffer.is_empty();
}
void push(const T &p_element) {
_buffer.push_back(p_element);
_indexer(p_element, _buffer.size() - 1);
_shift_up(_buffer.size() - 1);
}
T pop() {
ERR_FAIL_COND_V_MSG(_buffer.is_empty(), T(), "Can't pop an empty heap.");
T value = _buffer[0];
_indexer(value, INVALID_INDEX);
if (_buffer.size() > 1) {
_buffer[0] = _buffer[_buffer.size() - 1];
_indexer(_buffer[0], 0);
_buffer.remove_at(_buffer.size() - 1);
_shift_down(0);
} else {
_buffer.remove_at(_buffer.size() - 1);
}
return value;
}
/**
* Update the position of the element in the heap if necessary.
*/
void shift(uint32_t p_index) {
ERR_FAIL_UNSIGNED_INDEX_MSG(p_index, _buffer.size(), "Heap element index is out of range.");
if (!_shift_up(p_index)) {
_shift_down(p_index);
}
}
void clear() {
for (const T &value : _buffer) {
_indexer(value, INVALID_INDEX);
}
_buffer.clear();
}
Heap() {}
Heap(const LessThan &p_less_than) :
_less_than(p_less_than) {}
Heap(const Indexer &p_indexer) :
_indexer(p_indexer) {}
Heap(const LessThan &p_less_than, const Indexer &p_indexer) :
_less_than(p_less_than), _indexer(p_indexer) {}
private:
bool _shift_up(uint32_t p_index) {
T value = _buffer[p_index];
uint32_t current_index = p_index;
uint32_t parent_index = (current_index - 1) / 2;
while (current_index > 0 && _less_than(_buffer[parent_index], value)) {
_buffer[current_index] = _buffer[parent_index];
_indexer(_buffer[current_index], current_index);
current_index = parent_index;
parent_index = (current_index - 1) / 2;
}
if (current_index != p_index) {
_buffer[current_index] = value;
_indexer(value, current_index);
return true;
} else {
return false;
}
}
bool _shift_down(uint32_t p_index) {
T value = _buffer[p_index];
uint32_t current_index = p_index;
uint32_t child_index = 2 * current_index + 1;
while (child_index < _buffer.size()) {
if (child_index + 1 < _buffer.size() &&
_less_than(_buffer[child_index], _buffer[child_index + 1])) {
child_index++;
}
if (_less_than(_buffer[child_index], value)) {
break;
}
_buffer[current_index] = _buffer[child_index];
_indexer(_buffer[current_index], current_index);
current_index = child_index;
child_index = 2 * current_index + 1;
}
if (current_index != p_index) {
_buffer[current_index] = value;
_indexer(value, current_index);
return true;
} else {
return false;
}
}
};
struct EdgeConnectionPair {
gd::Edge::Connection connections[2];
Edge::Connection connections[2];
int size = 0;
};
@ -338,4 +213,4 @@ struct PerformanceData {
}
};
} // namespace gd
} // namespace nav_3d

158
servers/navigation/nav_heap.h Normal file
View File

@ -0,0 +1,158 @@
/**************************************************************************/
/* nav_heap.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#pragma once
#include "core/templates/local_vector.h"
template <typename T>
struct NoopIndexer {
void operator()(const T &p_value, uint32_t p_index) {}
};
/**
* A max-heap implementation that notifies of element index changes.
*/
template <typename T, typename LessThan = Comparator<T>, typename Indexer = NoopIndexer<T>>
class Heap {
LocalVector<T> _buffer;
LessThan _less_than;
Indexer _indexer;
public:
static constexpr uint32_t INVALID_INDEX = UINT32_MAX;
void reserve(uint32_t p_size) {
_buffer.reserve(p_size);
}
uint32_t size() const {
return _buffer.size();
}
bool is_empty() const {
return _buffer.is_empty();
}
void push(const T &p_element) {
_buffer.push_back(p_element);
_indexer(p_element, _buffer.size() - 1);
_shift_up(_buffer.size() - 1);
}
T pop() {
ERR_FAIL_COND_V_MSG(_buffer.is_empty(), T(), "Can't pop an empty heap.");
T value = _buffer[0];
_indexer(value, INVALID_INDEX);
if (_buffer.size() > 1) {
_buffer[0] = _buffer[_buffer.size() - 1];
_indexer(_buffer[0], 0);
_buffer.remove_at(_buffer.size() - 1);
_shift_down(0);
} else {
_buffer.remove_at(_buffer.size() - 1);
}
return value;
}
/**
* Update the position of the element in the heap if necessary.
*/
void shift(uint32_t p_index) {
ERR_FAIL_UNSIGNED_INDEX_MSG(p_index, _buffer.size(), "Heap element index is out of range.");
if (!_shift_up(p_index)) {
_shift_down(p_index);
}
}
void clear() {
for (const T &value : _buffer) {
_indexer(value, INVALID_INDEX);
}
_buffer.clear();
}
Heap() {}
Heap(const LessThan &p_less_than) :
_less_than(p_less_than) {}
Heap(const Indexer &p_indexer) :
_indexer(p_indexer) {}
Heap(const LessThan &p_less_than, const Indexer &p_indexer) :
_less_than(p_less_than), _indexer(p_indexer) {}
private:
bool _shift_up(uint32_t p_index) {
T value = _buffer[p_index];
uint32_t current_index = p_index;
uint32_t parent_index = (current_index - 1) / 2;
while (current_index > 0 && _less_than(_buffer[parent_index], value)) {
_buffer[current_index] = _buffer[parent_index];
_indexer(_buffer[current_index], current_index);
current_index = parent_index;
parent_index = (current_index - 1) / 2;
}
if (current_index != p_index) {
_buffer[current_index] = value;
_indexer(value, current_index);
return true;
} else {
return false;
}
}
bool _shift_down(uint32_t p_index) {
T value = _buffer[p_index];
uint32_t current_index = p_index;
uint32_t child_index = 2 * current_index + 1;
while (child_index < _buffer.size()) {
if (child_index + 1 < _buffer.size() &&
_less_than(_buffer[child_index], _buffer[child_index + 1])) {
child_index++;
}
if (_less_than(_buffer[child_index], value)) {
break;
}
_buffer[current_index] = _buffer[child_index];
_indexer(_buffer[current_index], current_index);
current_index = child_index;
child_index = 2 * current_index + 1;
}
if (current_index != p_index) {
_buffer[current_index] = value;
_indexer(value, current_index);
return true;
} else {
return false;
}
}
};

196
tests/servers/test_nav_heap.h Normal file
View File

@ -0,0 +1,196 @@
/**************************************************************************/
/* test_nav_heap.h */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/**************************************************************************/
#pragma once
#include "servers/navigation/nav_heap.h"
#include "tests/test_macros.h"
namespace TestHeap {
struct GreaterThan {
bool operator()(int p_a, int p_b) const { return p_a > p_b; }
};
struct CompareArrayValues {
const int *array;
CompareArrayValues(const int *p_array) :
array(p_array) {}
bool operator()(uint32_t p_index_a, uint32_t p_index_b) const {
return array[p_index_a] < array[p_index_b];
}
};
struct RegisterHeapIndexes {
uint32_t *indexes;
RegisterHeapIndexes(uint32_t *p_indexes) :
indexes(p_indexes) {}
void operator()(uint32_t p_vector_index, uint32_t p_heap_index) {
indexes[p_vector_index] = p_heap_index;
}
};
TEST_CASE("[Heap] size") {
Heap<int> heap;
CHECK(heap.size() == 0);
heap.push(0);
CHECK(heap.size() == 1);
heap.push(1);
CHECK(heap.size() == 2);
heap.pop();
CHECK(heap.size() == 1);
heap.pop();
CHECK(heap.size() == 0);
}
TEST_CASE("[Heap] is_empty") {
Heap<int> heap;
CHECK(heap.is_empty() == true);
heap.push(0);
CHECK(heap.is_empty() == false);
heap.pop();
CHECK(heap.is_empty() == true);
}
TEST_CASE("[Heap] push/pop") {
SUBCASE("Default comparator") {
Heap<int> heap;
heap.push(2);
heap.push(7);
heap.push(5);
heap.push(3);
heap.push(4);
CHECK(heap.pop() == 7);
CHECK(heap.pop() == 5);
CHECK(heap.pop() == 4);
CHECK(heap.pop() == 3);
CHECK(heap.pop() == 2);
}
SUBCASE("Custom comparator") {
GreaterThan greaterThan;
Heap<int, GreaterThan> heap(greaterThan);
heap.push(2);
heap.push(7);
heap.push(5);
heap.push(3);
heap.push(4);
CHECK(heap.pop() == 2);
CHECK(heap.pop() == 3);
CHECK(heap.pop() == 4);
CHECK(heap.pop() == 5);
CHECK(heap.pop() == 7);
}
SUBCASE("Intermediate pops") {
Heap<int> heap;
heap.push(0);
heap.push(3);
heap.pop();
heap.push(1);
heap.push(2);
CHECK(heap.pop() == 2);
CHECK(heap.pop() == 1);
CHECK(heap.pop() == 0);
}
}
TEST_CASE("[Heap] shift") {
int values[] = { 5, 3, 6, 7, 1 };
uint32_t heap_indexes[] = { 0, 0, 0, 0, 0 };
CompareArrayValues comparator(values);
RegisterHeapIndexes indexer(heap_indexes);
Heap<uint32_t, CompareArrayValues, RegisterHeapIndexes> heap(comparator, indexer);
heap.push(0);
heap.push(1);
heap.push(2);
heap.push(3);
heap.push(4);
// Shift down: 6 -> 2
values[2] = 2;
heap.shift(heap_indexes[2]);
// Shift up: 5 -> 8
values[0] = 8;
heap.shift(heap_indexes[0]);
CHECK(heap.pop() == 0);
CHECK(heap.pop() == 3);
CHECK(heap.pop() == 1);
CHECK(heap.pop() == 2);
CHECK(heap.pop() == 4);
CHECK(heap_indexes[0] == Heap<uint32_t, CompareArrayValues, RegisterHeapIndexes>::INVALID_INDEX);
CHECK(heap_indexes[1] == Heap<uint32_t, CompareArrayValues, RegisterHeapIndexes>::INVALID_INDEX);
CHECK(heap_indexes[2] == Heap<uint32_t, CompareArrayValues, RegisterHeapIndexes>::INVALID_INDEX);
CHECK(heap_indexes[3] == Heap<uint32_t, CompareArrayValues, RegisterHeapIndexes>::INVALID_INDEX);
CHECK(heap_indexes[4] == Heap<uint32_t, CompareArrayValues, RegisterHeapIndexes>::INVALID_INDEX);
}
TEST_CASE("[Heap] clear") {
uint32_t heap_indexes[] = { 0, 0, 0, 0 };
RegisterHeapIndexes indexer(heap_indexes);
Heap<uint32_t, Comparator<uint32_t>, RegisterHeapIndexes> heap(indexer);
heap.push(0);
heap.push(2);
heap.push(1);
heap.push(3);
heap.clear();
CHECK(heap.size() == 0);
CHECK(heap_indexes[0] == Heap<uint32_t, Comparator<uint32_t>, RegisterHeapIndexes>::INVALID_INDEX);
CHECK(heap_indexes[1] == Heap<uint32_t, Comparator<uint32_t>, RegisterHeapIndexes>::INVALID_INDEX);
CHECK(heap_indexes[2] == Heap<uint32_t, Comparator<uint32_t>, RegisterHeapIndexes>::INVALID_INDEX);
CHECK(heap_indexes[3] == Heap<uint32_t, Comparator<uint32_t>, RegisterHeapIndexes>::INVALID_INDEX);
}
} //namespace TestHeap

161
tests/servers/test_navigation_server_3d.h
View File

@ -34,8 +34,6 @@
#include "scene/resources/3d/primitive_meshes.h"
#include "servers/navigation_server_3d.h"
#include "modules/navigation/nav_utils.h"
namespace TestNavigationServer3D {
// TODO: Find a more generic way to create `Callable` mocks.
@ -62,32 +60,6 @@ static inline Array build_array(Variant item, Targs... Fargs) {
return a;
}
struct GreaterThan {
bool operator()(int p_a, int p_b) const { return p_a > p_b; }
};
struct CompareArrayValues {
const int *array;
CompareArrayValues(const int *p_array) :
array(p_array) {}
bool operator()(uint32_t p_index_a, uint32_t p_index_b) const {
return array[p_index_a] < array[p_index_b];
}
};
struct RegisterHeapIndexes {
uint32_t *indexes;
RegisterHeapIndexes(uint32_t *p_indexes) :
indexes(p_indexes) {}
void operator()(uint32_t p_vector_index, uint32_t p_heap_index) {
indexes[p_vector_index] = p_heap_index;
}
};
TEST_SUITE("[Navigation]") {
TEST_CASE("[NavigationServer3D] Server should be empty when initialized") {
NavigationServer3D *navigation_server = NavigationServer3D::get_singleton();
@ -881,138 +853,5 @@ TEST_SUITE("[Navigation]") {
Vector<Vector3> simplified_path = NavigationServer3D::get_singleton()->simplify_path(source_path, simplify_epsilon);
CHECK_EQ(simplified_path.size(), 4);
}
TEST_CASE("[Heap] size") {
gd::Heap<int> heap;
CHECK(heap.size() == 0);
heap.push(0);
CHECK(heap.size() == 1);
heap.push(1);
CHECK(heap.size() == 2);
heap.pop();
CHECK(heap.size() == 1);
heap.pop();
CHECK(heap.size() == 0);
}
TEST_CASE("[Heap] is_empty") {
gd::Heap<int> heap;
CHECK(heap.is_empty() == true);
heap.push(0);
CHECK(heap.is_empty() == false);
heap.pop();
CHECK(heap.is_empty() == true);
}
TEST_CASE("[Heap] push/pop") {
SUBCASE("Default comparator") {
gd::Heap<int> heap;
heap.push(2);
heap.push(7);
heap.push(5);
heap.push(3);
heap.push(4);
CHECK(heap.pop() == 7);
CHECK(heap.pop() == 5);
CHECK(heap.pop() == 4);
CHECK(heap.pop() == 3);
CHECK(heap.pop() == 2);
}
SUBCASE("Custom comparator") {
GreaterThan greaterThan;
gd::Heap<int, GreaterThan> heap(greaterThan);
heap.push(2);
heap.push(7);
heap.push(5);
heap.push(3);
heap.push(4);
CHECK(heap.pop() == 2);
CHECK(heap.pop() == 3);
CHECK(heap.pop() == 4);
CHECK(heap.pop() == 5);
CHECK(heap.pop() == 7);
}
SUBCASE("Intermediate pops") {
gd::Heap<int> heap;
heap.push(0);
heap.push(3);
heap.pop();
heap.push(1);
heap.push(2);
CHECK(heap.pop() == 2);
CHECK(heap.pop() == 1);
CHECK(heap.pop() == 0);
}
}
TEST_CASE("[Heap] shift") {
int values[] = { 5, 3, 6, 7, 1 };
uint32_t heap_indexes[] = { 0, 0, 0, 0, 0 };
CompareArrayValues comparator(values);
RegisterHeapIndexes indexer(heap_indexes);
gd::Heap<uint32_t, CompareArrayValues, RegisterHeapIndexes> heap(comparator, indexer);
heap.push(0);
heap.push(1);
heap.push(2);
heap.push(3);
heap.push(4);
// Shift down: 6 -> 2
values[2] = 2;
heap.shift(heap_indexes[2]);
// Shift up: 5 -> 8
values[0] = 8;
heap.shift(heap_indexes[0]);
CHECK(heap.pop() == 0);
CHECK(heap.pop() == 3);
CHECK(heap.pop() == 1);
CHECK(heap.pop() == 2);
CHECK(heap.pop() == 4);
CHECK(heap_indexes[0] == UINT32_MAX);
CHECK(heap_indexes[1] == UINT32_MAX);
CHECK(heap_indexes[2] == UINT32_MAX);
CHECK(heap_indexes[3] == UINT32_MAX);
CHECK(heap_indexes[4] == UINT32_MAX);
}
TEST_CASE("[Heap] clear") {
uint32_t heap_indexes[] = { 0, 0, 0, 0 };
RegisterHeapIndexes indexer(heap_indexes);
gd::Heap<uint32_t, Comparator<uint32_t>, RegisterHeapIndexes> heap(indexer);
heap.push(0);
heap.push(2);
heap.push(1);
heap.push(3);
heap.clear();
CHECK(heap.size() == 0);
CHECK(heap_indexes[0] == UINT32_MAX);
CHECK(heap_indexes[1] == UINT32_MAX);
CHECK(heap_indexes[2] == UINT32_MAX);
CHECK(heap_indexes[3] == UINT32_MAX);
}
}
} //namespace TestNavigationServer3D

1
tests/test_main.cpp
View File

@ -144,6 +144,7 @@
#include "tests/scene/test_visual_shader.h"
#include "tests/scene/test_window.h"
#include "tests/servers/rendering/test_shader_preprocessor.h"
#include "tests/servers/test_nav_heap.h"
#include "tests/servers/test_text_server.h"
#include "tests/test_validate_testing.h"