Sara/equipment-test
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

feat: updated engine version to 4.4-rc1

Browse source
This commit is contained in:
Sara 2025-02-23 14:38:14 +01:00
parent ee00efde1f
commit 21ba8e33af
5459 changed files with 1128836 additions and 198305 deletions
Download patch file Download diff file Expand all files Collapse all files

137
engine/modules/navigation/2d/godot_navigation_server_2d.cpp
View file

@ -81,12 +81,6 @@
return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1))); \
}
#define FORWARD_5_R_C(CONV_R, FUNC_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3, T_4, D_4, CONV_0, CONV_1, CONV_2, CONV_3, CONV_4) \
GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3, T_4 D_4) \
const { \
return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1), CONV_2(D_2), CONV_3(D_3), CONV_4(D_4))); \
}
static RID rid_to_rid(const RID d) {
return d;
}
@ -164,10 +158,19 @@ static Ref<NavigationMesh> poly_to_mesh(Ref<NavigationPolygon> d) {
}
}
static Rect2 aabb_to_rect2(AABB aabb) {
Rect2 rect2;
rect2.position = Vector2(aabb.position.x, aabb.position.z);
rect2.size = Vector2(aabb.size.x, aabb.size.z);
return rect2;
}
void GodotNavigationServer2D::init() {
#ifdef CLIPPER2_ENABLED
navmesh_generator_2d = memnew(NavMeshGenerator2D);
ERR_FAIL_NULL_MSG(navmesh_generator_2d, "Failed to init NavMeshGenerator2D.");
RWLockRead read_lock(geometry_parser_rwlock);
navmesh_generator_2d->set_generator_parsers(generator_parsers);
#endif // CLIPPER2_ENABLED
}
@ -191,7 +194,7 @@ void GodotNavigationServer2D::finish() {
void GodotNavigationServer2D::parse_source_geometry_data(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
ERR_FAIL_COND_MSG(!Thread::is_main_thread(), "The SceneTree can only be parsed on the main thread. Call this function from the main thread or use call_deferred().");
ERR_FAIL_COND_MSG(!p_navigation_mesh.is_valid(), "Invalid navigation polygon.");
ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation polygon.");
ERR_FAIL_NULL_MSG(p_root_node, "No parsing root node specified.");
ERR_FAIL_COND_MSG(!p_root_node->is_inside_tree(), "The root node needs to be inside the SceneTree.");
@ -202,8 +205,8 @@ void GodotNavigationServer2D::parse_source_geometry_data(const Ref<NavigationPol
}
void GodotNavigationServer2D::bake_from_source_geometry_data(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback) {
ERR_FAIL_COND_MSG(!p_navigation_mesh.is_valid(), "Invalid navigation polygon.");
ERR_FAIL_COND_MSG(!p_source_geometry_data.is_valid(), "Invalid NavigationMeshSourceGeometryData2D.");
ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation polygon.");
ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData2D.");
#ifdef CLIPPER2_ENABLED
ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
@ -212,8 +215,8 @@ void GodotNavigationServer2D::bake_from_source_geometry_data(const Ref<Navigatio
}
void GodotNavigationServer2D::bake_from_source_geometry_data_async(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback) {
ERR_FAIL_COND_MSG(!p_navigation_mesh.is_valid(), "Invalid navigation mesh.");
ERR_FAIL_COND_MSG(!p_source_geometry_data.is_valid(), "Invalid NavigationMeshSourceGeometryData2D.");
ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation mesh.");
ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData2D.");
#ifdef CLIPPER2_ENABLED
ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
@ -265,6 +268,14 @@ uint32_t GodotNavigationServer2D::map_get_iteration_id(RID p_map) const {
return NavigationServer3D::get_singleton()->map_get_iteration_id(p_map);
}
void GodotNavigationServer2D::map_set_use_async_iterations(RID p_map, bool p_enabled) {
return NavigationServer3D::get_singleton()->map_set_use_async_iterations(p_map, p_enabled);
}
bool GodotNavigationServer2D::map_get_use_async_iterations(RID p_map) const {
return NavigationServer3D::get_singleton()->map_get_use_async_iterations(p_map);
}
void FORWARD_2(map_set_cell_size, RID, p_map, real_t, p_cell_size, rid_to_rid, real_to_real);
real_t FORWARD_1_C(map_get_cell_size, RID, p_map, rid_to_rid);
@ -277,7 +288,9 @@ real_t FORWARD_1_C(map_get_edge_connection_margin, RID, p_map, rid_to_rid);
void FORWARD_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius, rid_to_rid, real_to_real);
real_t FORWARD_1_C(map_get_link_connection_radius, RID, p_map, rid_to_rid);
Vector<Vector2> FORWARD_5_R_C(vector_v3_to_v2, map_get_path, RID, p_map, Vector2, p_origin, Vector2, p_destination, bool, p_optimize, uint32_t, p_layers, rid_to_rid, v2_to_v3, v2_to_v3, bool_to_bool, uint32_to_uint32);
Vector<Vector2> GodotNavigationServer2D::map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers) {
return vector_v3_to_v2(NavigationServer3D::get_singleton()->map_get_path(p_map, v2_to_v3(p_origin), v2_to_v3(p_destination), p_optimize, p_navigation_layers));
}
Vector2 FORWARD_2_R_C(v3_to_v2, map_get_closest_point, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3);
RID FORWARD_2_C(map_get_closest_point_owner, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3);
@ -318,11 +331,21 @@ int FORWARD_1_C(region_get_connections_count, RID, p_region, rid_to_rid);
Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_start, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int);
Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_end, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int);
Vector2 GodotNavigationServer2D::region_get_closest_point(RID p_region, const Vector2 &p_point) const {
Vector3 result = NavigationServer3D::get_singleton()->region_get_closest_point(p_region, v2_to_v3(p_point));
return v3_to_v2(result);
}
Vector2 GodotNavigationServer2D::region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const {
Vector3 result = NavigationServer3D::get_singleton()->region_get_random_point(p_region, p_navigation_layers, p_uniformly);
return v3_to_v2(result);
}
Rect2 GodotNavigationServer2D::region_get_bounds(RID p_region) const {
AABB bounds = NavigationServer3D::get_singleton()->region_get_bounds(p_region);
return aabb_to_rect2(bounds);
}
RID FORWARD_0(link_create);
void FORWARD_2(link_set_map, RID, p_link, RID, p_map, rid_to_rid, rid_to_rid);
@ -390,12 +413,19 @@ void FORWARD_2(agent_set_paused, RID, p_agent, bool, p_paused, rid_to_rid, bool_
bool FORWARD_1_C(agent_get_paused, RID, p_agent, rid_to_rid);
void GodotNavigationServer2D::free(RID p_object) {
#ifdef CLIPPER2_ENABLED
if (navmesh_generator_2d && navmesh_generator_2d->owns(p_object)) {
navmesh_generator_2d->free(p_object);
if (geometry_parser_owner.owns(p_object)) {
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_object);
ERR_FAIL_NULL(parser);
generator_parsers.erase(parser);
#ifndef CLIPPER2_ENABLED
NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
#endif
geometry_parser_owner.free(parser->self);
return;
}
#endif // CLIPPER2_ENABLED
NavigationServer3D::get_singleton()->free(p_object);
}
@ -451,31 +481,70 @@ Vector<Vector2> GodotNavigationServer2D::obstacle_get_vertices(RID p_obstacle) c
return vector_v3_to_v2(NavigationServer3D::get_singleton()->obstacle_get_vertices(p_obstacle));
}
void GodotNavigationServer2D::query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const {
ERR_FAIL_COND(!p_query_parameters.is_valid());
ERR_FAIL_COND(!p_query_result.is_valid());
void GodotNavigationServer2D::query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback) {
ERR_FAIL_COND(p_query_parameters.is_null());
ERR_FAIL_COND(p_query_result.is_null());
const NavigationUtilities::PathQueryResult _query_result = NavigationServer3D::get_singleton()->_query_path(p_query_parameters->get_parameters());
Ref<NavigationPathQueryParameters3D> query_parameters;
query_parameters.instantiate();
p_query_result->set_path(vector_v3_to_v2(_query_result.path));
p_query_result->set_path_types(_query_result.path_types);
p_query_result->set_path_rids(_query_result.path_rids);
p_query_result->set_path_owner_ids(_query_result.path_owner_ids);
query_parameters->set_map(p_query_parameters->get_map());
query_parameters->set_start_position(v2_to_v3(p_query_parameters->get_start_position()));
query_parameters->set_target_position(v2_to_v3(p_query_parameters->get_target_position()));
query_parameters->set_navigation_layers(p_query_parameters->get_navigation_layers());
query_parameters->set_pathfinding_algorithm(NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR);
switch (p_query_parameters->get_path_postprocessing()) {
case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
} break;
case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED);
} break;
case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_NONE: {
query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_NONE);
} break;
default: {
WARN_PRINT("No match for used PathPostProcessing - fallback to default");
query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
} break;
}
query_parameters->set_metadata_flags((int64_t)p_query_parameters->get_metadata_flags());
query_parameters->set_simplify_path(p_query_parameters->get_simplify_path());
query_parameters->set_simplify_epsilon(p_query_parameters->get_simplify_epsilon());
Ref<NavigationPathQueryResult3D> query_result;
query_result.instantiate();
NavigationServer3D::get_singleton()->query_path(query_parameters, query_result, p_callback);
p_query_result->set_path(vector_v3_to_v2(query_result->get_path()));
p_query_result->set_path_types(query_result->get_path_types());
p_query_result->set_path_rids(query_result->get_path_rids());
p_query_result->set_path_owner_ids(query_result->get_path_owner_ids());
}
RID GodotNavigationServer2D::source_geometry_parser_create() {
RWLockWrite write_lock(geometry_parser_rwlock);
RID rid = geometry_parser_owner.make_rid();
NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(rid);
parser->self = rid;
generator_parsers.push_back(parser);
#ifdef CLIPPER2_ENABLED
if (navmesh_generator_2d) {
return navmesh_generator_2d->source_geometry_parser_create();
}
#endif // CLIPPER2_ENABLED
return RID();
NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
#endif
return rid;
}
void GodotNavigationServer2D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
#ifdef CLIPPER2_ENABLED
if (navmesh_generator_2d) {
navmesh_generator_2d->source_geometry_parser_set_callback(p_parser, p_callback);
}
#endif // CLIPPER2_ENABLED
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_parser);
ERR_FAIL_NULL(parser);
parser->callback = p_callback;
}

8
engine/modules/navigation/2d/godot_navigation_server_2d.h
View file

@ -68,7 +68,7 @@ public:
virtual real_t map_get_edge_connection_margin(RID p_map) const override;
virtual void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override;
virtual real_t map_get_link_connection_radius(RID p_map) const override;
virtual Vector<Vector2> map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const override;
virtual Vector<Vector2> map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override;
virtual Vector2 map_get_closest_point(RID p_map, const Vector2 &p_point) const override;
virtual RID map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const override;
virtual TypedArray<RID> map_get_links(RID p_map) const override;
@ -78,6 +78,8 @@ public:
virtual void map_force_update(RID p_map) override;
virtual Vector2 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const override;
virtual uint32_t map_get_iteration_id(RID p_map) const override;
virtual void map_set_use_async_iterations(RID p_map, bool p_enabled) override;
virtual bool map_get_use_async_iterations(RID p_map) const override;
virtual RID region_create() override;
virtual void region_set_enabled(RID p_region, bool p_enabled) override;
@ -101,7 +103,9 @@ public:
virtual int region_get_connections_count(RID p_region) const override;
virtual Vector2 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override;
virtual Vector2 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override;
virtual Vector2 region_get_closest_point(RID p_region, const Vector2 &p_point) const override;
virtual Vector2 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const override;
virtual Rect2 region_get_bounds(RID p_region) const override;
virtual RID link_create() override;
@ -241,7 +245,7 @@ public:
virtual void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override;
virtual uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const override;
virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const override;
virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback) override;
virtual void init() override;
virtual void sync() override;

844
engine/modules/navigation/2d/nav_mesh_generator_2d.cpp
View file

File diff suppressed because it is too large Load diff

23
engine/modules/navigation/2d/nav_mesh_generator_2d.h
View file

@ -36,6 +36,7 @@
#include "core/object/class_db.h"
#include "core/object/worker_thread_pool.h"
#include "core/templates/rid_owner.h"
#include "servers/navigation_server_2d.h"
class Node;
class NavigationPolygon;
@ -47,12 +48,7 @@ class NavMeshGenerator2D : public Object {
static Mutex baking_navmesh_mutex;
static Mutex generator_task_mutex;
static RWLock generator_rid_rwlock;
struct NavMeshGeometryParser2D {
RID self;
Callable callback;
};
static RID_Owner<NavMeshGeometryParser2D> generator_parser_owner;
static RWLock generator_parsers_rwlock;
static LocalVector<NavMeshGeometryParser2D *> generator_parsers;
static bool use_threads;
@ -85,13 +81,6 @@ class NavMeshGenerator2D : public Object {
static void generator_parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node);
static void generator_bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data);
static void generator_parse_meshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static void generator_parse_multimeshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static void generator_parse_polygon2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static void generator_parse_staticbody2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static void generator_parse_tile_map_layer_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static void generator_parse_navigationobstacle_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static bool generator_emit_callback(const Callable &p_callback);
public:
@ -101,17 +90,13 @@ public:
static void cleanup();
static void finish();
static void set_generator_parsers(LocalVector<NavMeshGeometryParser2D *> p_parsers);
static void parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
static void bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback = Callable());
static void bake_from_source_geometry_data_async(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback = Callable());
static bool is_baking(Ref<NavigationPolygon> p_navigation_polygon);
static RID source_geometry_parser_create();
static void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback);
static bool owns(RID p_object);
static void free(RID p_object);
NavMeshGenerator2D();
~NavMeshGenerator2D();
};

314
engine/modules/navigation/3d/godot_navigation_server_3d.cpp
View file

@ -237,11 +237,29 @@ real_t GodotNavigationServer3D::map_get_link_connection_radius(RID p_map) const
return map->get_link_connection_radius();
}
Vector<Vector3> GodotNavigationServer3D::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) const {
Vector<Vector3> GodotNavigationServer3D::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers) {
const NavMap *map = map_owner.get_or_null(p_map);
ERR_FAIL_NULL_V(map, Vector<Vector3>());
return map->get_path(p_origin, p_destination, p_optimize, p_navigation_layers, nullptr, nullptr, nullptr);
Ref<NavigationPathQueryParameters3D> query_parameters;
query_parameters.instantiate();
query_parameters->set_map(p_map);
query_parameters->set_start_position(p_origin);
query_parameters->set_target_position(p_destination);
query_parameters->set_navigation_layers(p_navigation_layers);
query_parameters->set_pathfinding_algorithm(NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR);
query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
if (!p_optimize) {
query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PATH_POSTPROCESSING_EDGECENTERED);
}
Ref<NavigationPathQueryResult3D> query_result;
query_result.instantiate();
query_path(query_parameters, query_result);
return query_result->get_path();
}
Vector3 GodotNavigationServer3D::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
@ -346,6 +364,19 @@ RID GodotNavigationServer3D::agent_get_map(RID p_agent) const {
return RID();
}
COMMAND_2(map_set_use_async_iterations, RID, p_map, bool, p_enabled) {
NavMap *map = map_owner.get_or_null(p_map);
ERR_FAIL_NULL(map);
map->set_use_async_iterations(p_enabled);
}
bool GodotNavigationServer3D::map_get_use_async_iterations(RID p_map) const {
const NavMap *map = map_owner.get_or_null(p_map);
ERR_FAIL_NULL_V(map, false);
return map->get_use_async_iterations();
}
Vector3 GodotNavigationServer3D::map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const {
const NavMap *map = map_owner.get_or_null(p_map);
ERR_FAIL_NULL_V(map, Vector3());
@ -509,22 +540,52 @@ void GodotNavigationServer3D::region_bake_navigation_mesh(Ref<NavigationMesh> p_
int GodotNavigationServer3D::region_get_connections_count(RID p_region) const {
NavRegion *region = region_owner.get_or_null(p_region);
ERR_FAIL_NULL_V(region, 0);
return region->get_connections_count();
NavMap *map = region->get_map();
if (map) {
return map->get_region_connections_count(region);
}
return 0;
}
Vector3 GodotNavigationServer3D::region_get_connection_pathway_start(RID p_region, int p_connection_id) const {
NavRegion *region = region_owner.get_or_null(p_region);
ERR_FAIL_NULL_V(region, Vector3());
return region->get_connection_pathway_start(p_connection_id);
NavMap *map = region->get_map();
if (map) {
return map->get_region_connection_pathway_start(region, p_connection_id);
}
return Vector3();
}
Vector3 GodotNavigationServer3D::region_get_connection_pathway_end(RID p_region, int p_connection_id) const {
NavRegion *region = region_owner.get_or_null(p_region);
ERR_FAIL_NULL_V(region, Vector3());
NavMap *map = region->get_map();
if (map) {
return map->get_region_connection_pathway_end(region, p_connection_id);
}
return Vector3();
}
return region->get_connection_pathway_end(p_connection_id);
Vector3 GodotNavigationServer3D::region_get_closest_point_to_segment(RID p_region, const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const {
const NavRegion *region = region_owner.get_or_null(p_region);
ERR_FAIL_NULL_V(region, Vector3());
return region->get_closest_point_to_segment(p_from, p_to, p_use_collision);
}
Vector3 GodotNavigationServer3D::region_get_closest_point(RID p_region, const Vector3 &p_point) const {
const NavRegion *region = region_owner.get_or_null(p_region);
ERR_FAIL_NULL_V(region, Vector3());
return region->get_closest_point_info(p_point).point;
}
Vector3 GodotNavigationServer3D::region_get_closest_point_normal(RID p_region, const Vector3 &p_point) const {
const NavRegion *region = region_owner.get_or_null(p_region);
ERR_FAIL_NULL_V(region, Vector3());
return region->get_closest_point_info(p_point).normal;
}
Vector3 GodotNavigationServer3D::region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const {
@ -534,6 +595,13 @@ Vector3 GodotNavigationServer3D::region_get_random_point(RID p_region, uint32_t
return region->get_random_point(p_navigation_layers, p_uniformly);
}
AABB GodotNavigationServer3D::region_get_bounds(RID p_region) const {
const NavRegion *region = region_owner.get_or_null(p_region);
ERR_FAIL_NULL_V(region, AABB());
return region->get_bounds();
}
RID GodotNavigationServer3D::link_create() {
MutexLock lock(operations_mutex);
@ -1102,7 +1170,7 @@ uint32_t GodotNavigationServer3D::obstacle_get_avoidance_layers(RID p_obstacle)
void GodotNavigationServer3D::parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
#ifndef _3D_DISABLED
ERR_FAIL_COND_MSG(!Thread::is_main_thread(), "The SceneTree can only be parsed on the main thread. Call this function from the main thread or use call_deferred().");
ERR_FAIL_COND_MSG(!p_navigation_mesh.is_valid(), "Invalid navigation mesh.");
ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation mesh.");
ERR_FAIL_NULL_MSG(p_root_node, "No parsing root node specified.");
ERR_FAIL_COND_MSG(!p_root_node->is_inside_tree(), "The root node needs to be inside the SceneTree.");
@ -1113,8 +1181,8 @@ void GodotNavigationServer3D::parse_source_geometry_data(const Ref<NavigationMes
void GodotNavigationServer3D::bake_from_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback) {
#ifndef _3D_DISABLED
ERR_FAIL_COND_MSG(!p_navigation_mesh.is_valid(), "Invalid navigation mesh.");
ERR_FAIL_COND_MSG(!p_source_geometry_data.is_valid(), "Invalid NavigationMeshSourceGeometryData3D.");
ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation mesh.");
ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData3D.");
ERR_FAIL_NULL(NavMeshGenerator3D::get_singleton());
NavMeshGenerator3D::get_singleton()->bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
@ -1123,8 +1191,8 @@ void GodotNavigationServer3D::bake_from_source_geometry_data(const Ref<Navigatio
void GodotNavigationServer3D::bake_from_source_geometry_data_async(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback) {
#ifndef _3D_DISABLED
ERR_FAIL_COND_MSG(!p_navigation_mesh.is_valid(), "Invalid navigation mesh.");
ERR_FAIL_COND_MSG(!p_source_geometry_data.is_valid(), "Invalid NavigationMeshSourceGeometryData3D.");
ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation mesh.");
ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData3D.");
ERR_FAIL_NULL(NavMeshGenerator3D::get_singleton());
NavMeshGenerator3D::get_singleton()->bake_from_source_geometry_data_async(p_navigation_mesh, p_source_geometry_data, p_callback);
@ -1202,10 +1270,17 @@ COMMAND_1(free, RID, p_object) {
} else if (obstacle_owner.owns(p_object)) {
internal_free_obstacle(p_object);
} else if (geometry_parser_owner.owns(p_object)) {
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(p_object);
ERR_FAIL_NULL(parser);
generator_parsers.erase(parser);
#ifndef _3D_DISABLED
} else if (navmesh_generator_3d && navmesh_generator_3d->owns(p_object)) {
navmesh_generator_3d->free(p_object);
#endif // _3D_DISABLED
NavMeshGenerator3D::get_singleton()->set_generator_parsers(generator_parsers);
#endif
geometry_parser_owner.free(parser->self);
} else {
ERR_PRINT("Attempted to free a NavigationServer RID that did not exist (or was already freed).");
@ -1298,6 +1373,7 @@ void GodotNavigationServer3D::process(real_t p_delta_time) {
int _new_pm_edge_merge_count = 0;
int _new_pm_edge_connection_count = 0;
int _new_pm_edge_free_count = 0;
int _new_pm_obstacle_count = 0;
// In c++ we can't be sure that this is performed in the main thread
// even with mutable functions.
@ -1315,6 +1391,7 @@ void GodotNavigationServer3D::process(real_t p_delta_time) {
_new_pm_edge_merge_count += active_maps[i]->get_pm_edge_merge_count();
_new_pm_edge_connection_count += active_maps[i]->get_pm_edge_connection_count();
_new_pm_edge_free_count += active_maps[i]->get_pm_edge_free_count();
_new_pm_obstacle_count += active_maps[i]->get_pm_obstacle_count();
// Emit a signal if a map changed.
const uint32_t new_map_iteration_id = active_maps[i]->get_iteration_id();
@ -1332,11 +1409,14 @@ void GodotNavigationServer3D::process(real_t p_delta_time) {
pm_edge_merge_count = _new_pm_edge_merge_count;
pm_edge_connection_count = _new_pm_edge_connection_count;
pm_edge_free_count = _new_pm_edge_free_count;
pm_obstacle_count = _new_pm_obstacle_count;
}
void GodotNavigationServer3D::init() {
#ifndef _3D_DISABLED
navmesh_generator_3d = memnew(NavMeshGenerator3D);
RWLockRead read_lock(geometry_parser_rwlock);
navmesh_generator_3d->set_generator_parsers(generator_parsers);
#endif // _3D_DISABLED
}
@ -1351,103 +1431,38 @@ void GodotNavigationServer3D::finish() {
#endif // _3D_DISABLED
}
PathQueryResult GodotNavigationServer3D::_query_path(const PathQueryParameters &p_parameters) const {
PathQueryResult r_query_result;
void GodotNavigationServer3D::query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback) {
ERR_FAIL_COND(p_query_parameters.is_null());
ERR_FAIL_COND(p_query_result.is_null());
const NavMap *map = map_owner.get_or_null(p_parameters.map);
ERR_FAIL_NULL_V(map, r_query_result);
NavMap *map = map_owner.get_or_null(p_query_parameters->get_map());
ERR_FAIL_NULL(map);
// run the pathfinding
if (p_parameters.pathfinding_algorithm == PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR) {
// while postprocessing is still part of map.get_path() need to check and route it here for the correct "optimize" post-processing
if (p_parameters.path_postprocessing == PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL) {
r_query_result.path = map->get_path(
p_parameters.start_position,
p_parameters.target_position,
true,
p_parameters.navigation_layers,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES) ? &r_query_result.path_types : nullptr,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS) ? &r_query_result.path_rids : nullptr,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS) ? &r_query_result.path_owner_ids : nullptr);
} else if (p_parameters.path_postprocessing == PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED) {
r_query_result.path = map->get_path(
p_parameters.start_position,
p_parameters.target_position,
false,
p_parameters.navigation_layers,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES) ? &r_query_result.path_types : nullptr,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS) ? &r_query_result.path_rids : nullptr,
p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS) ? &r_query_result.path_owner_ids : nullptr);
}
} else {
return r_query_result;
}
// add path postprocessing
if (r_query_result.path.size() > 2 && p_parameters.simplify_path) {
const LocalVector<uint32_t> &simplified_path_indices = get_simplified_path_indices(r_query_result.path, p_parameters.simplify_epsilon);
uint32_t indices_count = simplified_path_indices.size();
{
Vector3 *w = r_query_result.path.ptrw();
const Vector3 *r = r_query_result.path.ptr();
for (uint32_t i = 0; i < indices_count; i++) {
w[i] = r[simplified_path_indices[i]];
}
r_query_result.path.resize(indices_count);
}
if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
int32_t *w = r_query_result.path_types.ptrw();
const int32_t *r = r_query_result.path_types.ptr();
for (uint32_t i = 0; i < indices_count; i++) {
w[i] = r[simplified_path_indices[i]];
}
r_query_result.path_types.resize(indices_count);
}
if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
TypedArray<RID> simplified_path_rids;
simplified_path_rids.resize(indices_count);
for (uint32_t i = 0; i < indices_count; i++) {
simplified_path_rids[i] = r_query_result.path_rids[i];
}
r_query_result.path_rids = simplified_path_rids;
}
if (p_parameters.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
int64_t *w = r_query_result.path_owner_ids.ptrw();
const int64_t *r = r_query_result.path_owner_ids.ptr();
for (uint32_t i = 0; i < indices_count; i++) {
w[i] = r[simplified_path_indices[i]];
}
r_query_result.path_owner_ids.resize(indices_count);
}
}
// add path stats
return r_query_result;
NavMeshQueries3D::map_query_path(map, p_query_parameters, p_query_result, p_callback);
}
RID GodotNavigationServer3D::source_geometry_parser_create() {
RWLockWrite write_lock(geometry_parser_rwlock);
RID rid = geometry_parser_owner.make_rid();
NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(rid);
parser->self = rid;
generator_parsers.push_back(parser);
#ifndef _3D_DISABLED
if (navmesh_generator_3d) {
return navmesh_generator_3d->source_geometry_parser_create();
}
#endif // _3D_DISABLED
return RID();
NavMeshGenerator3D::get_singleton()->set_generator_parsers(generator_parsers);
#endif
return rid;
}
void GodotNavigationServer3D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
#ifndef _3D_DISABLED
if (navmesh_generator_3d) {
navmesh_generator_3d->source_geometry_parser_set_callback(p_parser, p_callback);
}
#endif // _3D_DISABLED
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(p_parser);
ERR_FAIL_NULL(parser);
parser->callback = p_callback;
}
Vector<Vector3> GodotNavigationServer3D::simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) {
@ -1457,86 +1472,32 @@ Vector<Vector3> GodotNavigationServer3D::simplify_path(const Vector<Vector3> &p_
p_epsilon = MAX(0.0, p_epsilon);
LocalVector<uint32_t> simplified_path_indices = get_simplified_path_indices(p_path, p_epsilon);
LocalVector<Vector3> source_path;
{
source_path.resize(p_path.size());
const Vector3 *r = p_path.ptr();
for (uint32_t i = 0; i < p_path.size(); i++) {
source_path[i] = r[i];
}
}
uint32_t indices_count = simplified_path_indices.size();
LocalVector<uint32_t> simplified_path_indices = NavMeshQueries3D::get_simplified_path_indices(source_path, p_epsilon);
uint32_t index_count = simplified_path_indices.size();
Vector<Vector3> simplified_path;
simplified_path.resize(indices_count);
Vector3 *w = simplified_path.ptrw();
const Vector3 *r = p_path.ptr();
for (uint32_t i = 0; i < indices_count; i++) {
w[i] = r[simplified_path_indices[i]];
{
simplified_path.resize(index_count);
Vector3 *w = simplified_path.ptrw();
const Vector3 *r = source_path.ptr();
for (uint32_t i = 0; i < index_count; i++) {
w[i] = r[simplified_path_indices[i]];
}
}
return simplified_path;
}
LocalVector<uint32_t> GodotNavigationServer3D::get_simplified_path_indices(const Vector<Vector3> &p_path, real_t p_epsilon) {
p_epsilon = MAX(0.0, p_epsilon);
real_t squared_epsilon = p_epsilon * p_epsilon;
LocalVector<bool> valid_points;
valid_points.resize(p_path.size());
for (uint32_t i = 0; i < valid_points.size(); i++) {
valid_points[i] = false;
}
simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, valid_points);
int valid_point_index = 0;
for (bool valid : valid_points) {
if (valid) {
valid_point_index += 1;
}
}
LocalVector<uint32_t> simplified_path_indices;
simplified_path_indices.resize(valid_point_index);
valid_point_index = 0;
for (uint32_t i = 0; i < valid_points.size(); i++) {
if (valid_points[i]) {
simplified_path_indices[valid_point_index] = i;
valid_point_index += 1;
}
}
return simplified_path_indices;
}
void GodotNavigationServer3D::simplify_path_segment(int p_start_inx, int p_end_inx, const Vector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points) {
r_valid_points[p_start_inx] = true;
r_valid_points[p_end_inx] = true;
const Vector3 &start_point = p_points[p_start_inx];
const Vector3 &end_point = p_points[p_end_inx];
Vector3 path_segment[2] = { start_point, end_point };
real_t point_max_distance = 0.0;
int point_max_index = 0;
for (int i = p_start_inx; i < p_end_inx; i++) {
const Vector3 &checked_point = p_points[i];
const Vector3 closest_point = Geometry3D::get_closest_point_to_segment(checked_point, path_segment);
real_t distance_squared = closest_point.distance_squared_to(checked_point);
if (distance_squared > point_max_distance) {
point_max_index = i;
point_max_distance = distance_squared;
}
}
if (point_max_distance > p_epsilon) {
simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_valid_points);
simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_valid_points);
}
}
int GodotNavigationServer3D::get_process_info(ProcessInfo p_info) const {
switch (p_info) {
case INFO_ACTIVE_MAPS: {
@ -1566,6 +1527,9 @@ int GodotNavigationServer3D::get_process_info(ProcessInfo p_info) const {
case INFO_EDGE_FREE_COUNT: {
return pm_edge_free_count;
} break;
case INFO_OBSTACLE_COUNT: {
return pm_obstacle_count;
} break;
}
return 0;

18
engine/modules/navigation/3d/godot_navigation_server_3d.h
View file

@ -40,6 +40,8 @@
#include "core/templates/local_vector.h"
#include "core/templates/rid.h"
#include "core/templates/rid_owner.h"
#include "servers/navigation/navigation_path_query_parameters_3d.h"
#include "servers/navigation/navigation_path_query_result_3d.h"
#include "servers/navigation_server_3d.h"
/// The commands are functions executed during the `sync` phase.
@ -95,6 +97,7 @@ class GodotNavigationServer3D : public NavigationServer3D {
int pm_edge_merge_count = 0;
int pm_edge_connection_count = 0;
int pm_edge_free_count = 0;
int pm_obstacle_count = 0;
public:
GodotNavigationServer3D();
@ -129,7 +132,7 @@ public:
COMMAND_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius);
virtual real_t map_get_link_connection_radius(RID p_map) const override;
virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) const override;
virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override;
virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const override;
virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const override;
@ -144,6 +147,9 @@ public:
virtual void map_force_update(RID p_map) override;
virtual uint32_t map_get_iteration_id(RID p_map) const override;
COMMAND_2(map_set_use_async_iterations, RID, p_map, bool, p_enabled);
virtual bool map_get_use_async_iterations(RID p_map) const override;
virtual Vector3 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const override;
virtual RID region_create() override;
@ -177,7 +183,11 @@ public:
virtual int region_get_connections_count(RID p_region) const override;
virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override;
virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override;
virtual Vector3 region_get_closest_point_to_segment(RID p_region, const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision = false) const override;
virtual Vector3 region_get_closest_point(RID p_region, const Vector3 &p_point) const override;
virtual Vector3 region_get_closest_point_normal(RID p_region, const Vector3 &p_point) const override;
virtual Vector3 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const override;
virtual AABB region_get_bounds(RID p_region) const override;
virtual RID link_create() override;
COMMAND_2(link_set_map, RID, p_link, RID, p_map);
@ -269,10 +279,6 @@ public:
virtual Vector<Vector3> simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) override;
private:
static void simplify_path_segment(int p_start_inx, int p_end_inx, const Vector<Vector3> &p_points, real_t p_epsilon, LocalVector<bool> &r_valid_points);
static LocalVector<uint32_t> get_simplified_path_indices(const Vector<Vector3> &p_path, real_t p_epsilon);
public:
COMMAND_1(free, RID, p_object);
@ -284,7 +290,7 @@ public:
virtual void sync() override;
virtual void finish() override;
virtual NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const override;
virtual void query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback = Callable()) override;
int get_process_info(ProcessInfo p_info) const override;

57
engine/modules/navigation/3d/nav_base_iteration_3d.h Normal file
View file

@ -0,0 +1,57 @@
/**************************************************************************/
/* nav_base_iteration_3d.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. */
/**************************************************************************/
#ifndef NAV_BASE_ITERATION_3D_H
#define NAV_BASE_ITERATION_3D_H
#include "servers/navigation/navigation_utilities.h"
struct NavBaseIteration {
uint32_t id = UINT32_MAX;
bool enabled = true;
uint32_t navigation_layers = 1;
real_t enter_cost = 0.0;
real_t travel_cost = 1.0;
NavigationUtilities::PathSegmentType owner_type;
ObjectID owner_object_id;
RID owner_rid;
bool owner_use_edge_connections = false;
bool get_enabled() const { return enabled; }
NavigationUtilities::PathSegmentType get_type() const { return owner_type; }
RID get_self() const { return owner_rid; }
ObjectID get_owner_id() const { return owner_object_id; }
uint32_t get_navigation_layers() const { return navigation_layers; }
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; }
};
#endif // NAV_BASE_ITERATION_3D_H

409
engine/modules/navigation/3d/nav_map_builder_3d.cpp Normal file
View file

@ -0,0 +1,409 @@
/**************************************************************************/
/* nav_map_builder_3d.cpp */
/**************************************************************************/
/* 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. */
/**************************************************************************/
#ifndef _3D_DISABLED
#include "nav_map_builder_3d.h"
#include "../nav_link.h"
#include "../nav_map.h"
#include "../nav_region.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) {
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;
p.key = 0;
p.x = x;
p.y = y;
p.z = z;
return p;
}
void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild &r_build) {
gd::PerformanceData &performance_data = r_build.performance_data;
performance_data.pm_polygon_count = 0;
performance_data.pm_edge_count = 0;
performance_data.pm_edge_merge_count = 0;
performance_data.pm_edge_connection_count = 0;
performance_data.pm_edge_free_count = 0;
_build_step_gather_region_polygons(r_build);
_build_step_find_edge_connection_pairs(r_build);
_build_step_merge_edge_connection_pairs(r_build);
_build_step_edge_connection_margin_connections(r_build);
_build_step_navlink_connections(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;
LocalVector<NavRegionIteration> &regions = map_iteration->region_iterations;
HashMap<uint32_t, LocalVector<gd::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>();
}
// Copy all region polygons in the map.
int polygon_count = 0;
for (NavRegionIteration &region : regions) {
if (!region.get_enabled()) {
continue;
}
LocalVector<gd::Polygon> &polygons_source = region.navmesh_polygons;
for (uint32_t n = 0; n < polygons_source.size(); n++) {
polygons_source[n].id = polygon_count;
polygon_count++;
}
}
performance_data.pm_polygon_count = polygon_count;
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;
int polygon_count = r_build.polygon_count;
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::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) {
if (!region.get_enabled()) {
continue;
}
for (gd::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);
HashMap<gd::EdgeKey, gd::EdgeConnectionPair, gd::EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
if (!pair_it) {
pair_it = connection_pairs_map.insert(ek, gd::EdgeConnectionPair());
performance_data.pm_edge_count += 1;
++free_edges_count;
}
gd::EdgeConnectionPair &pair = pair_it->value;
if (pair.size < 2) {
// Add the polygon/edge tuple to this key.
gd::Edge::Connection new_connection;
new_connection.polygon = &poly;
new_connection.edge = p;
new_connection.pathway_start = poly.points[p].pos;
new_connection.pathway_end = poly.points[next_point].pos;
pair.connections[pair.size] = new_connection;
++pair.size;
if (pair.size == 2) {
--free_edges_count;
}
} else {
// The edge is already connected with another edge, skip.
ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001.");
}
}
}
}
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;
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;
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;
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];
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.
performance_data.pm_edge_merge_count += 1;
} else {
CRASH_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size));
if (use_edge_connections && pair.connections[0].polygon->owner->get_use_edge_connections()) {
free_edges.push_back(pair.connections[0]);
}
}
}
}
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;
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;
// Find the compatible near edges.
//
// Note:
// Considering that the edges must be compatible (for obvious reasons)
// to be connected, create new polygons to remove that small gap is
// not really useful and would result in wasteful computation during
// connection, integration and path finding.
performance_data.pm_edge_free_count = free_edges.size();
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];
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];
if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
continue;
}
Vector3 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos;
Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
// Compute the projection of the opposite edge on the current one
Vector3 edge_vector = edge_p2 - edge_p1;
real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector.length_squared());
real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector.length_squared());
if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
continue;
}
// Check if the two edges are close to each other enough and compute a pathway between the two regions.
Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
Vector3 other1;
if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) {
other1 = other_edge_p1;
} else {
other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
}
if (other1.distance_squared_to(self1) > edge_connection_margin_squared) {
continue;
}
Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
Vector3 other2;
if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) {
other2 = other_edge_p2;
} else {
other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
}
if (other2.distance_squared_to(self2) > edge_connection_margin_squared) {
continue;
}
// The edges can now be connected.
gd::Edge::Connection new_connection = other_edge;
new_connection.pathway_start = (self1 + other1) / 2.0;
new_connection.pathway_end = (self2 + other2) / 2.0;
free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
// Add the connection to the region_connection map.
region_external_connections[(uint32_t)free_edge.polygon->owner->id].push_back(new_connection);
performance_data.pm_edge_connection_count += 1;
}
}
}
void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild &r_build) {
NavMapIteration *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;
int polygon_count = r_build.polygon_count;
real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius;
uint32_t link_poly_idx = 0;
link_polygons.resize(links.size());
// Search for polygons within range of a nav link.
for (const NavLinkIteration &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;
real_t closest_start_sqr_dist = link_connection_radius_sqr;
Vector3 closest_start_point;
gd::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) {
if (!region.get_enabled()) {
continue;
}
AABB region_bounds = region.get_bounds().grow(link_connection_radius);
if (!region_bounds.has_point(link_start_pos) && !region_bounds.has_point(link_end_pos)) {
continue;
}
for (gd::Polygon &polyon : region.navmesh_polygons) {
//for (gd::Polygon &polyon : 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);
{
const Vector3 start_point = face.get_closest_point_to(link_start_pos);
const real_t sqr_dist = start_point.distance_squared_to(link_start_pos);
// Pick the polygon that is within our radius and is closer than anything we've seen yet.
if (sqr_dist < closest_start_sqr_dist) {
closest_start_sqr_dist = sqr_dist;
closest_start_point = start_point;
closest_start_polygon = &polyon;
}
}
{
const Vector3 end_point = face.get_closest_point_to(link_end_pos);
const real_t sqr_dist = end_point.distance_squared_to(link_end_pos);
// Pick the polygon that is within our radius and is closer than anything we've seen yet.
if (sqr_dist < closest_end_sqr_dist) {
closest_end_sqr_dist = sqr_dist;
closest_end_point = end_point;
closest_end_polygon = &polyon;
}
}
}
}
}
// 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++];
new_polygon.id = polygon_count++;
new_polygon.owner = &link;
new_polygon.edges.clear();
new_polygon.edges.resize(4);
new_polygon.points.resize(4);
// Build a set of vertices that create a thin polygon going from the start to the end point.
new_polygon.points[0] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) };
new_polygon.points[1] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) };
new_polygon.points[2] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) };
new_polygon.points[3] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) };
// Setup connections to go forward in the link.
{
gd::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;
exit_connection.polygon = closest_end_polygon;
exit_connection.edge = -1;
exit_connection.pathway_start = new_polygon.points[2].pos;
exit_connection.pathway_end = new_polygon.points[3].pos;
new_polygon.edges[2].connections.push_back(exit_connection);
}
// If the link is bi-directional, create connections from the end to the start.
if (link.is_bidirectional()) {
gd::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;
exit_connection.polygon = closest_start_polygon;
exit_connection.edge = -1;
exit_connection.pathway_start = new_polygon.points[0].pos;
exit_connection.pathway_end = new_polygon.points[1].pos;
new_polygon.edges[0].connections.push_back(exit_connection);
}
}
}
}
void NavMapBuilder3D::_build_update_map_iteration(NavMapIterationBuild &r_build) {
NavMapIteration *map_iteration = r_build.map_iteration;
LocalVector<gd::Polygon> &link_polygons = map_iteration->link_polygons;
map_iteration->navmesh_polygon_count = r_build.polygon_count;
map_iteration->link_polygon_count = link_polygons.size();
map_iteration->path_query_slots_mutex.lock();
for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : map_iteration->path_query_slots) {
p_path_query_slot.traversable_polys.clear();
p_path_query_slot.traversable_polys.reserve(map_iteration->navmesh_polygon_count * 0.25);
p_path_query_slot.path_corridor.clear();
p_path_query_slot.path_corridor.resize(map_iteration->navmesh_polygon_count + map_iteration->link_polygon_count);
}
map_iteration->path_query_slots_mutex.unlock();
}
#endif // _3D_DISABLED

52
engine/modules/navigation/3d/nav_map_builder_3d.h Normal file
View file

@ -0,0 +1,52 @@
/**************************************************************************/
/* nav_map_builder_3d.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. */
/**************************************************************************/
#ifndef NAV_MAP_BUILDER_3D_H
#define NAV_MAP_BUILDER_3D_H
#include "../nav_utils.h"
struct NavMapIterationBuild;
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);
public:
static gd::PointKey get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size);
static void build_navmap_iteration(NavMapIterationBuild &r_build);
};
#endif // NAV_MAP_BUILDER_3D_H

114
engine/modules/navigation/3d/nav_map_iteration_3d.h Normal file
View file

@ -0,0 +1,114 @@
/**************************************************************************/
/* nav_map_iteration_3d.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. */
/**************************************************************************/
#ifndef NAV_MAP_ITERATION_3D_H
#define NAV_MAP_ITERATION_3D_H
#include "../nav_rid.h"
#include "../nav_utils.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 NavMapIterationBuild {
Vector3 merge_rasterizer_cell_size;
bool use_edge_connections = true;
real_t edge_connection_margin;
real_t link_connection_radius;
gd::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;
NavMapIteration *map_iteration = nullptr;
int navmesh_polygon_count = 0;
int link_polygon_count = 0;
void reset() {
performance_data.reset();
iter_connection_pairs_map.clear();
iter_free_edges.clear();
polygon_count = 0;
free_edge_count = 0;
navmesh_polygon_count = 0;
link_polygon_count = 0;
}
};
struct NavMapIteration {
mutable SafeNumeric<uint32_t> users;
RWLock rwlock;
Vector3 map_up;
LocalVector<gd::Polygon> link_polygons;
LocalVector<NavRegionIteration> region_iterations;
LocalVector<NavLinkIteration> 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<NavRegion *, 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;
public:
_ALWAYS_INLINE_ NavMapIterationRead(const NavMapIteration &p_iteration) :
map_iteration(p_iteration) {
map_iteration.rwlock.read_lock();
map_iteration.users.increment();
}
_ALWAYS_INLINE_ ~NavMapIterationRead() {
map_iteration.users.decrement();
map_iteration.rwlock.read_unlock();
}
};
#endif // NAV_MAP_ITERATION_3D_H

544
engine/modules/navigation/3d/nav_mesh_generator_3d.cpp
View file

@ -33,47 +33,23 @@
#include "nav_mesh_generator_3d.h"
#include "core/config/project_settings.h"
#include "core/math/convex_hull.h"
#include "core/os/thread.h"
#include "scene/3d/mesh_instance_3d.h"
#include "scene/3d/multimesh_instance_3d.h"
#include "scene/3d/navigation_obstacle_3d.h"
#include "scene/3d/physics/static_body_3d.h"
#include "scene/resources/3d/box_shape_3d.h"
#include "scene/resources/3d/capsule_shape_3d.h"
#include "scene/resources/3d/concave_polygon_shape_3d.h"
#include "scene/resources/3d/convex_polygon_shape_3d.h"
#include "scene/resources/3d/cylinder_shape_3d.h"
#include "scene/resources/3d/height_map_shape_3d.h"
#include "scene/3d/node_3d.h"
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
#include "scene/resources/3d/primitive_meshes.h"
#include "scene/resources/3d/shape_3d.h"
#include "scene/resources/3d/sphere_shape_3d.h"
#include "scene/resources/3d/world_boundary_shape_3d.h"
#include "scene/resources/navigation_mesh.h"
#include "modules/modules_enabled.gen.h" // For csg, gridmap.
#ifdef MODULE_CSG_ENABLED
#include "modules/csg/csg_shape.h"
#endif
#ifdef MODULE_GRIDMAP_ENABLED
#include "modules/gridmap/grid_map.h"
#endif
#include <Recast.h>
NavMeshGenerator3D *NavMeshGenerator3D::singleton = nullptr;
Mutex NavMeshGenerator3D::baking_navmesh_mutex;
Mutex NavMeshGenerator3D::generator_task_mutex;
RWLock NavMeshGenerator3D::generator_rid_rwlock;
RWLock NavMeshGenerator3D::generator_parsers_rwlock;
bool NavMeshGenerator3D::use_threads = true;
bool NavMeshGenerator3D::baking_use_multiple_threads = true;
bool NavMeshGenerator3D::baking_use_high_priority_threads = true;
HashSet<Ref<NavigationMesh>> NavMeshGenerator3D::baking_navmeshes;
HashMap<WorkerThreadPool::TaskID, NavMeshGenerator3D::NavMeshGeneratorTask3D *> NavMeshGenerator3D::generator_tasks;
RID_Owner<NavMeshGenerator3D::NavMeshGeometryParser3D> NavMeshGenerator3D::generator_parser_owner;
LocalVector<NavMeshGenerator3D::NavMeshGeometryParser3D *> NavMeshGenerator3D::generator_parsers;
LocalVector<NavMeshGeometryParser3D *> NavMeshGenerator3D::generator_parsers;
NavMeshGenerator3D *NavMeshGenerator3D::get_singleton() {
return singleton;
@ -100,57 +76,52 @@ void NavMeshGenerator3D::sync() {
return;
}
baking_navmesh_mutex.lock();
generator_task_mutex.lock();
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
{
MutexLock generator_task_lock(generator_task_mutex);
LocalVector<WorkerThreadPool::TaskID> finished_task_ids;
LocalVector<WorkerThreadPool::TaskID> finished_task_ids;
for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) {
if (WorkerThreadPool::get_singleton()->is_task_completed(E.key)) {
WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
finished_task_ids.push_back(E.key);
for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) {
if (WorkerThreadPool::get_singleton()->is_task_completed(E.key)) {
WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
finished_task_ids.push_back(E.key);
NavMeshGeneratorTask3D *generator_task = E.value;
DEV_ASSERT(generator_task->status == NavMeshGeneratorTask3D::TaskStatus::BAKING_FINISHED);
NavMeshGeneratorTask3D *generator_task = E.value;
DEV_ASSERT(generator_task->status == NavMeshGeneratorTask3D::TaskStatus::BAKING_FINISHED);
baking_navmeshes.erase(generator_task->navigation_mesh);
if (generator_task->callback.is_valid()) {
generator_emit_callback(generator_task->callback);
baking_navmeshes.erase(generator_task->navigation_mesh);
if (generator_task->callback.is_valid()) {
generator_emit_callback(generator_task->callback);
}
memdelete(generator_task);
}
memdelete(generator_task);
}
for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) {
generator_tasks.erase(finished_task_id);
}
}
for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) {
generator_tasks.erase(finished_task_id);
}
generator_task_mutex.unlock();
baking_navmesh_mutex.unlock();
}
void NavMeshGenerator3D::cleanup() {
baking_navmesh_mutex.lock();
generator_task_mutex.lock();
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
{
MutexLock generator_task_lock(generator_task_mutex);
baking_navmeshes.clear();
baking_navmeshes.clear();
for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) {
WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
NavMeshGeneratorTask3D *generator_task = E.value;
memdelete(generator_task);
for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) {
WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
NavMeshGeneratorTask3D *generator_task = E.value;
memdelete(generator_task);
}
generator_tasks.clear();
generator_parsers_rwlock.write_lock();
generator_parsers.clear();
generator_parsers_rwlock.write_unlock();
}
generator_tasks.clear();
generator_rid_rwlock.write_lock();
for (NavMeshGeometryParser3D *parser : generator_parsers) {
generator_parser_owner.free(parser->self);
}
generator_parsers.clear();
generator_rid_rwlock.write_unlock();
generator_task_mutex.unlock();
baking_navmesh_mutex.unlock();
}
void NavMeshGenerator3D::finish() {
@ -159,10 +130,10 @@ void NavMeshGenerator3D::finish() {
void NavMeshGenerator3D::parse_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
ERR_FAIL_COND(!Thread::is_main_thread());
ERR_FAIL_COND(!p_navigation_mesh.is_valid());
ERR_FAIL_COND(p_navigation_mesh.is_null());
ERR_FAIL_NULL(p_root_node);
ERR_FAIL_COND(!p_root_node->is_inside_tree());
ERR_FAIL_COND(!p_source_geometry_data.is_valid());
ERR_FAIL_COND(p_source_geometry_data.is_null());
generator_parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node);
@ -172,8 +143,8 @@ void NavMeshGenerator3D::parse_source_geometry_data(Ref<NavigationMesh> p_naviga
}
void NavMeshGenerator3D::bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback) {
ERR_FAIL_COND(!p_navigation_mesh.is_valid());
ERR_FAIL_COND(!p_source_geometry_data.is_valid());
ERR_FAIL_COND(p_navigation_mesh.is_null());
ERR_FAIL_COND(p_source_geometry_data.is_null());
if (!p_source_geometry_data->has_data()) {
p_navigation_mesh->clear();
@ -202,8 +173,8 @@ void NavMeshGenerator3D::bake_from_source_geometry_data(Ref<NavigationMesh> p_na
}
void NavMeshGenerator3D::bake_from_source_geometry_data_async(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback) {
ERR_FAIL_COND(!p_navigation_mesh.is_valid());
ERR_FAIL_COND(!p_source_geometry_data.is_valid());
ERR_FAIL_COND(p_navigation_mesh.is_null());
ERR_FAIL_COND(p_source_geometry_data.is_null());
if (!p_source_geometry_data->has_data()) {
p_navigation_mesh->clear();
@ -226,7 +197,7 @@ void NavMeshGenerator3D::bake_from_source_geometry_data_async(Ref<NavigationMesh
baking_navmeshes.insert(p_navigation_mesh);
baking_navmesh_mutex.unlock();
generator_task_mutex.lock();
MutexLock generator_task_lock(generator_task_mutex);
NavMeshGeneratorTask3D *generator_task = memnew(NavMeshGeneratorTask3D);
generator_task->navigation_mesh = p_navigation_mesh;
generator_task->source_geometry_data = p_source_geometry_data;
@ -234,14 +205,11 @@ void NavMeshGenerator3D::bake_from_source_geometry_data_async(Ref<NavigationMesh
generator_task->status = NavMeshGeneratorTask3D::TaskStatus::BAKING_STARTED;
generator_task->thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMeshGenerator3D::generator_thread_bake, generator_task, NavMeshGenerator3D::baking_use_high_priority_threads, SNAME("NavMeshGeneratorBake3D"));
generator_tasks.insert(generator_task->thread_task_id, generator_task);
generator_task_mutex.unlock();
}
bool NavMeshGenerator3D::is_baking(Ref<NavigationMesh> p_navigation_mesh) {
baking_navmesh_mutex.lock();
bool baking = baking_navmeshes.has(p_navigation_mesh);
baking_navmesh_mutex.unlock();
return baking;
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
return baking_navmeshes.has(p_navigation_mesh);
}
void NavMeshGenerator3D::generator_thread_bake(void *p_arg) {
@ -253,25 +221,14 @@ void NavMeshGenerator3D::generator_thread_bake(void *p_arg) {
}
void NavMeshGenerator3D::generator_parse_geometry_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node, bool p_recurse_children) {
generator_parse_meshinstance3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_parse_multimeshinstance3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_parse_staticbody3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
#ifdef MODULE_CSG_ENABLED
generator_parse_csgshape3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
#endif
#ifdef MODULE_GRIDMAP_ENABLED
generator_parse_gridmap_node(p_navigation_mesh, p_source_geometry_data, p_node);
#endif
generator_parse_navigationobstacle_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_rid_rwlock.read_lock();
generator_parsers_rwlock.read_lock();
for (const NavMeshGeometryParser3D *parser : generator_parsers) {
if (!parser->callback.is_valid()) {
continue;
}
parser->callback.call(p_navigation_mesh, p_source_geometry_data, p_node);
}
generator_rid_rwlock.read_unlock();
generator_parsers_rwlock.read_unlock();
if (p_recurse_children) {
for (int i = 0; i < p_node->get_child_count(); i++) {
@ -280,367 +237,9 @@ void NavMeshGenerator3D::generator_parse_geometry_node(const Ref<NavigationMesh>
}
}
void NavMeshGenerator3D::generator_parse_meshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
MeshInstance3D *mesh_instance = Object::cast_to<MeshInstance3D>(p_node);
if (mesh_instance) {
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
Ref<Mesh> mesh = mesh_instance->get_mesh();
if (mesh.is_valid()) {
p_source_geometry_data->add_mesh(mesh, mesh_instance->get_global_transform());
}
}
}
}
void NavMeshGenerator3D::generator_parse_multimeshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
MultiMeshInstance3D *multimesh_instance = Object::cast_to<MultiMeshInstance3D>(p_node);
if (multimesh_instance) {
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
Ref<MultiMesh> multimesh = multimesh_instance->get_multimesh();
if (multimesh.is_valid()) {
Ref<Mesh> mesh = multimesh->get_mesh();
if (mesh.is_valid()) {
int n = multimesh->get_visible_instance_count();
if (n == -1) {
n = multimesh->get_instance_count();
}
for (int i = 0; i < n; i++) {
p_source_geometry_data->add_mesh(mesh, multimesh_instance->get_global_transform() * multimesh->get_instance_transform(i));
}
}
}
}
}
}
void NavMeshGenerator3D::generator_parse_staticbody3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
StaticBody3D *static_body = Object::cast_to<StaticBody3D>(p_node);
if (static_body) {
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (static_body->get_collision_layer() & parsed_collision_mask)) {
List<uint32_t> shape_owners;
static_body->get_shape_owners(&shape_owners);
for (uint32_t shape_owner : shape_owners) {
if (static_body->is_shape_owner_disabled(shape_owner)) {
continue;
}
const int shape_count = static_body->shape_owner_get_shape_count(shape_owner);
for (int shape_index = 0; shape_index < shape_count; shape_index++) {
Ref<Shape3D> s = static_body->shape_owner_get_shape(shape_owner, shape_index);
if (s.is_null()) {
continue;
}
const Transform3D transform = static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner);
BoxShape3D *box = Object::cast_to<BoxShape3D>(*s);
if (box) {
Array arr;
arr.resize(RS::ARRAY_MAX);
BoxMesh::create_mesh_array(arr, box->get_size());
p_source_geometry_data->add_mesh_array(arr, transform);
}
CapsuleShape3D *capsule = Object::cast_to<CapsuleShape3D>(*s);
if (capsule) {
Array arr;
arr.resize(RS::ARRAY_MAX);
CapsuleMesh::create_mesh_array(arr, capsule->get_radius(), capsule->get_height());
p_source_geometry_data->add_mesh_array(arr, transform);
}
CylinderShape3D *cylinder = Object::cast_to<CylinderShape3D>(*s);
if (cylinder) {
Array arr;
arr.resize(RS::ARRAY_MAX);
CylinderMesh::create_mesh_array(arr, cylinder->get_radius(), cylinder->get_radius(), cylinder->get_height());
p_source_geometry_data->add_mesh_array(arr, transform);
}
SphereShape3D *sphere = Object::cast_to<SphereShape3D>(*s);
if (sphere) {
Array arr;
arr.resize(RS::ARRAY_MAX);
SphereMesh::create_mesh_array(arr, sphere->get_radius(), sphere->get_radius() * 2.0);
p_source_geometry_data->add_mesh_array(arr, transform);
}
ConcavePolygonShape3D *concave_polygon = Object::cast_to<ConcavePolygonShape3D>(*s);
if (concave_polygon) {
p_source_geometry_data->add_faces(concave_polygon->get_faces(), transform);
}
ConvexPolygonShape3D *convex_polygon = Object::cast_to<ConvexPolygonShape3D>(*s);
if (convex_polygon) {
Vector<Vector3> varr = Variant(convex_polygon->get_points());
Geometry3D::MeshData md;
Error err = ConvexHullComputer::convex_hull(varr, md);
if (err == OK) {
PackedVector3Array faces;
for (const Geometry3D::MeshData::Face &face : md.faces) {
for (uint32_t k = 2; k < face.indices.size(); ++k) {
faces.push_back(md.vertices[face.indices[0]]);
faces.push_back(md.vertices[face.indices[k - 1]]);
faces.push_back(md.vertices[face.indices[k]]);
}
}
p_source_geometry_data->add_faces(faces, transform);
}
}
HeightMapShape3D *heightmap_shape = Object::cast_to<HeightMapShape3D>(*s);
if (heightmap_shape) {
int heightmap_depth = heightmap_shape->get_map_depth();
int heightmap_width = heightmap_shape->get_map_width();
if (heightmap_depth >= 2 && heightmap_width >= 2) {
const Vector<real_t> &map_data = heightmap_shape->get_map_data();
Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1);
Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5;
Vector<Vector3> vertex_array;
vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6);
Vector3 *vertex_array_ptrw = vertex_array.ptrw();
const real_t *map_data_ptr = map_data.ptr();
int vertex_index = 0;
for (int d = 0; d < heightmap_depth - 1; d++) {
for (int w = 0; w < heightmap_width - 1; w++) {
vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d);
vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1);
vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_index += 6;
}
}
if (vertex_array.size() > 0) {
p_source_geometry_data->add_faces(vertex_array, transform);
}
}
}
}
}
}
}
}
#ifdef MODULE_CSG_ENABLED
void NavMeshGenerator3D::generator_parse_csgshape3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
CSGShape3D *csgshape3d = Object::cast_to<CSGShape3D>(p_node);
if (csgshape3d) {
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS && csgshape3d->is_using_collision() && (csgshape3d->get_collision_layer() & parsed_collision_mask)) || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
CSGShape3D *csg_shape = Object::cast_to<CSGShape3D>(p_node);
Array meshes = csg_shape->get_meshes();
if (!meshes.is_empty()) {
Ref<Mesh> mesh = meshes[1];
if (mesh.is_valid()) {
p_source_geometry_data->add_mesh(mesh, csg_shape->get_global_transform());
}
}
}
}
}
#endif // MODULE_CSG_ENABLED
#ifdef MODULE_GRIDMAP_ENABLED
void NavMeshGenerator3D::generator_parse_gridmap_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
GridMap *gridmap = Object::cast_to<GridMap>(p_node);
if (gridmap) {
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
Array meshes = gridmap->get_meshes();
Transform3D xform = gridmap->get_global_transform();
for (int i = 0; i < meshes.size(); i += 2) {
Ref<Mesh> mesh = meshes[i + 1];
if (mesh.is_valid()) {
p_source_geometry_data->add_mesh(mesh, xform * (Transform3D)meshes[i]);
}
}
}
else if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (gridmap->get_collision_layer() & parsed_collision_mask)) {
Array shapes = gridmap->get_collision_shapes();
for (int i = 0; i < shapes.size(); i += 2) {
RID shape = shapes[i + 1];
PhysicsServer3D::ShapeType type = PhysicsServer3D::get_singleton()->shape_get_type(shape);
Variant data = PhysicsServer3D::get_singleton()->shape_get_data(shape);
switch (type) {
case PhysicsServer3D::SHAPE_SPHERE: {
real_t radius = data;
Array arr;
arr.resize(RS::ARRAY_MAX);
SphereMesh::create_mesh_array(arr, radius, radius * 2.0);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_BOX: {
Vector3 extents = data;
Array arr;
arr.resize(RS::ARRAY_MAX);
BoxMesh::create_mesh_array(arr, extents * 2.0);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_CAPSULE: {
Dictionary dict = data;
real_t radius = dict["radius"];
real_t height = dict["height"];
Array arr;
arr.resize(RS::ARRAY_MAX);
CapsuleMesh::create_mesh_array(arr, radius, height);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_CYLINDER: {
Dictionary dict = data;
real_t radius = dict["radius"];
real_t height = dict["height"];
Array arr;
arr.resize(RS::ARRAY_MAX);
CylinderMesh::create_mesh_array(arr, radius, radius, height);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_CONVEX_POLYGON: {
PackedVector3Array vertices = data;
Geometry3D::MeshData md;
Error err = ConvexHullComputer::convex_hull(vertices, md);
if (err == OK) {
PackedVector3Array faces;
for (const Geometry3D::MeshData::Face &face : md.faces) {
for (uint32_t k = 2; k < face.indices.size(); ++k) {
faces.push_back(md.vertices[face.indices[0]]);
faces.push_back(md.vertices[face.indices[k - 1]]);
faces.push_back(md.vertices[face.indices[k]]);
}
}
p_source_geometry_data->add_faces(faces, shapes[i]);
}
} break;
case PhysicsServer3D::SHAPE_CONCAVE_POLYGON: {
Dictionary dict = data;
PackedVector3Array faces = Variant(dict["faces"]);
p_source_geometry_data->add_faces(faces, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_HEIGHTMAP: {
Dictionary dict = data;
///< dict( int:"width", int:"depth",float:"cell_size", float_array:"heights"
int heightmap_depth = dict["depth"];
int heightmap_width = dict["width"];
if (heightmap_depth >= 2 && heightmap_width >= 2) {
const Vector<real_t> &map_data = dict["heights"];
Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1);
Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5;
Vector<Vector3> vertex_array;
vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6);
Vector3 *vertex_array_ptrw = vertex_array.ptrw();
const real_t *map_data_ptr = map_data.ptr();
int vertex_index = 0;
for (int d = 0; d < heightmap_depth - 1; d++) {
for (int w = 0; w < heightmap_width - 1; w++) {
vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d);
vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1);
vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_index += 6;
}
}
if (vertex_array.size() > 0) {
p_source_geometry_data->add_faces(vertex_array, shapes[i]);
}
}
} break;
default: {
WARN_PRINT("Unsupported collision shape type.");
} break;
}
}
}
}
}
#endif // MODULE_GRIDMAP_ENABLED
void NavMeshGenerator3D::generator_parse_navigationobstacle_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
NavigationObstacle3D *obstacle = Object::cast_to<NavigationObstacle3D>(p_node);
if (obstacle == nullptr) {
return;
}
if (!obstacle->get_affect_navigation_mesh()) {
return;
}
const Transform3D node_xform = p_source_geometry_data->root_node_transform * Transform3D(Basis(), obstacle->get_global_position());
const float obstacle_radius = obstacle->get_radius();
if (obstacle_radius > 0.0) {
Vector<Vector3> obstruction_circle_vertices;
// The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding.
// Without, navigation paths can go directly through the middle of the obstacle and conflict with the avoidance to get agents stuck.
// No place for excessive "round" detail here. Every additional edge adds a high cost for something that needs to be quick, not pretty.
static const int circle_points = 12;
obstruction_circle_vertices.resize(circle_points);
Vector3 *circle_vertices_ptrw = obstruction_circle_vertices.ptrw();
const real_t circle_point_step = Math_TAU / circle_points;
for (int i = 0; i < circle_points; i++) {
const float angle = i * circle_point_step;
circle_vertices_ptrw[i] = node_xform.xform(Vector3(Math::cos(angle) * obstacle_radius, 0.0, Math::sin(angle) * obstacle_radius));
}
p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y - obstacle_radius, obstacle_radius, obstacle->get_carve_navigation_mesh());
}
const Vector<Vector3> &obstacle_vertices = obstacle->get_vertices();
if (obstacle_vertices.is_empty()) {
return;
}
Vector<Vector3> obstruction_shape_vertices;
obstruction_shape_vertices.resize(obstacle_vertices.size());
const Vector3 *obstacle_vertices_ptr = obstacle_vertices.ptr();
Vector3 *obstruction_shape_vertices_ptrw = obstruction_shape_vertices.ptrw();
for (int i = 0; i < obstacle_vertices.size(); i++) {
obstruction_shape_vertices_ptrw[i] = node_xform.xform(obstacle_vertices_ptr[i]);
obstruction_shape_vertices_ptrw[i].y = 0.0;
}
p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y, obstacle->get_height(), obstacle->get_carve_navigation_mesh());
void NavMeshGenerator3D::set_generator_parsers(LocalVector<NavMeshGeometryParser3D *> p_parsers) {
RWLockWrite write_lock(generator_parsers_rwlock);
generator_parsers = p_parsers;
}
void NavMeshGenerator3D::generator_parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node) {
@ -665,7 +264,7 @@ void NavMeshGenerator3D::generator_parse_source_geometry_data(const Ref<Navigati
for (Node *parse_node : parse_nodes) {
generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, parse_node, recurse_children);
}
};
}
void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data) {
if (p_navigation_mesh.is_null() || p_source_geometry_data.is_null()) {
@ -962,45 +561,4 @@ bool NavMeshGenerator3D::generator_emit_callback(const Callable &p_callback) {
return ce.error == Callable::CallError::CALL_OK;
}
RID NavMeshGenerator3D::source_geometry_parser_create() {
RWLockWrite write_lock(generator_rid_rwlock);
RID rid = generator_parser_owner.make_rid();
NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(rid);
parser->self = rid;
generator_parsers.push_back(parser);
return rid;
}
void NavMeshGenerator3D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
RWLockWrite write_lock(generator_rid_rwlock);
NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(p_parser);
ERR_FAIL_NULL(parser);
parser->callback = p_callback;
}
bool NavMeshGenerator3D::owns(RID p_object) {
RWLockRead read_lock(generator_rid_rwlock);
return generator_parser_owner.owns(p_object);
}
void NavMeshGenerator3D::free(RID p_object) {
RWLockWrite write_lock(generator_rid_rwlock);
if (generator_parser_owner.owns(p_object)) {
NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(p_object);
generator_parsers.erase(parser);
generator_parser_owner.free(p_object);
} else {
ERR_PRINT("Attempted to free a NavMeshGenerator3D RID that did not exist (or was already freed).");
}
}
#endif // _3D_DISABLED

28
engine/modules/navigation/3d/nav_mesh_generator_3d.h
View file

@ -36,7 +36,7 @@
#include "core/object/class_db.h"
#include "core/object/worker_thread_pool.h"
#include "core/templates/rid_owner.h"
#include "modules/modules_enabled.gen.h" // For csg, gridmap.
#include "servers/navigation_server_3d.h"
class Node;
class NavigationMesh;
@ -48,12 +48,7 @@ class NavMeshGenerator3D : public Object {
static Mutex baking_navmesh_mutex;
static Mutex generator_task_mutex;
static RWLock generator_rid_rwlock;
struct NavMeshGeometryParser3D {
RID self;
Callable callback;
};
static RID_Owner<NavMeshGeometryParser3D> generator_parser_owner;
static RWLock generator_parsers_rwlock;
static LocalVector<NavMeshGeometryParser3D *> generator_parsers;
static bool use_threads;
@ -86,17 +81,6 @@ class NavMeshGenerator3D : public Object {
static void generator_parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node);
static void generator_bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data);
static void generator_parse_meshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
static void generator_parse_multimeshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
static void generator_parse_staticbody3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
#ifdef MODULE_CSG_ENABLED
static void generator_parse_csgshape3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
#endif // MODULE_CSG_ENABLED
#ifdef MODULE_GRIDMAP_ENABLED
static void generator_parse_gridmap_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
#endif // MODULE_GRIDMAP_ENABLED
static void generator_parse_navigationobstacle_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
static bool generator_emit_callback(const Callable &p_callback);
public:
@ -106,17 +90,13 @@ public:
static void cleanup();
static void finish();
static void set_generator_parsers(LocalVector<NavMeshGeometryParser3D *> p_parsers);
static void parse_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
static void bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback = Callable());
static void bake_from_source_geometry_data_async(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback = Callable());
static bool is_baking(Ref<NavigationMesh> p_navigation_mesh);
static RID source_geometry_parser_create();
static void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback);
static bool owns(RID p_object);
static void free(RID p_object);
NavMeshGenerator3D();
~NavMeshGenerator3D();
};

1130
engine/modules/navigation/3d/nav_mesh_queries_3d.cpp Normal file
View file

File diff suppressed because it is too large Load diff

148
engine/modules/navigation/3d/nav_mesh_queries_3d.h Normal file
View file

@ -0,0 +1,148 @@
/**************************************************************************/
/* nav_mesh_queries_3d.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. */
/**************************************************************************/
#ifndef NAV_MESH_QUERIES_3D_H
#define NAV_MESH_QUERIES_3D_H
#ifndef _3D_DISABLED
#include "../nav_utils.h"
#include "servers/navigation/navigation_path_query_parameters_3d.h"
#include "servers/navigation/navigation_path_query_result_3d.h"
#include "servers/navigation/navigation_utilities.h"
using namespace NavigationUtilities;
class NavMap;
struct NavMapIteration;
class NavMeshQueries3D {
public:
struct PathQuerySlot {
LocalVector<gd::NavigationPoly> path_corridor;
gd::Heap<gd::NavigationPoly *, gd::NavPolyTravelCostGreaterThan, gd::NavPolyHeapIndexer> traversable_polys;
bool in_use = false;
uint32_t slot_index = 0;
};
struct NavMeshPathQueryTask3D {
enum TaskStatus {
QUERY_STARTED,
QUERY_FINISHED,
QUERY_FAILED,
CALLBACK_DISPATCHED,
CALLBACK_FAILED,
};
// Parameters.
Vector3 start_position;
Vector3 target_position;
uint32_t navigation_layers;
BitField<PathMetadataFlags> metadata_flags = PathMetadataFlags::PATH_INCLUDE_ALL;
PathfindingAlgorithm pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
PathPostProcessing path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
bool simplify_path = false;
real_t simplify_epsilon = 0.0;
// Path building.
Vector3 begin_position;
Vector3 end_position;
const gd::Polygon *begin_polygon = nullptr;
const gd::Polygon *end_polygon = nullptr;
uint32_t least_cost_id = 0;
// Map.
Vector3 map_up;
NavMap *map = nullptr;
PathQuerySlot *path_query_slot = nullptr;
// Path points.
LocalVector<Vector3> path_points;
LocalVector<int32_t> path_meta_point_types;
LocalVector<RID> path_meta_point_rids;
LocalVector<int64_t> path_meta_point_owners;
Ref<NavigationPathQueryParameters3D> query_parameters;
Ref<NavigationPathQueryResult3D> query_result;
Callable callback;
NavMeshPathQueryTask3D::TaskStatus status = NavMeshPathQueryTask3D::TaskStatus::QUERY_STARTED;
void path_clear() {
path_points.clear();
path_meta_point_types.clear();
path_meta_point_rids.clear();
path_meta_point_owners.clear();
}
void path_reverse() {
path_points.invert();
path_meta_point_types.invert();
path_meta_point_rids.invert();
path_meta_point_owners.invert();
}
};
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_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 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 void map_query_path(NavMap *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_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_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);
static LocalVector<uint32_t> get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon);
};
#endif // _3D_DISABLED
#endif // NAV_MESH_QUERIES_3D_H

51
engine/modules/navigation/3d/nav_region_iteration_3d.h Normal file
View file

@ -0,0 +1,51 @@
/**************************************************************************/
/* nav_region_iteration_3d.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. */
/**************************************************************************/
#ifndef NAV_REGION_ITERATION_3D_H
#define NAV_REGION_ITERATION_3D_H
#include "../nav_utils.h"
#include "nav_base_iteration_3d.h"
#include "core/math/aabb.h"
struct NavRegionIteration : NavBaseIteration {
Transform3D transform;
LocalVector<gd::Polygon> navmesh_polygons;
real_t surface_area = 0.0;
AABB bounds;
const Transform3D &get_transform() const { return transform; }
const LocalVector<gd::Polygon> &get_navmesh_polygons() const { return navmesh_polygons; }
real_t get_surface_area() const { return surface_area; }
AABB get_bounds() const { return bounds; }
};
#endif // NAV_REGION_ITERATION_3D_H

1
engine/modules/navigation/SCsub
View file

@ -1,4 +1,5 @@
#!/usr/bin/env python
from misc.utility.scons_hints import *
Import("env")
Import("env_modules")

1
engine/modules/navigation/config.py
View file

@ -1,4 +1,5 @@
def can_build(env, platform):
env.module_add_dependencies("navigation", ["csg", "gridmap"], True)
return not env["disable_3d"]

18
engine/modules/navigation/editor/navigation_mesh_editor_plugin.cpp
View file

@ -32,11 +32,8 @@
#ifdef TOOLS_ENABLED
#include "core/io/marshalls.h"
#include "core/io/resource_saver.h"
#include "editor/editor_node.h"
#include "editor/editor_string_names.h"
#include "scene/3d/mesh_instance_3d.h"
#include "scene/3d/navigation_region_3d.h"
#include "scene/gui/box_container.h"
#include "scene/gui/button.h"
@ -54,8 +51,8 @@ void NavigationMeshEditor::_node_removed(Node *p_node) {
void NavigationMeshEditor::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_ENTER_TREE: {
button_bake->set_icon(get_theme_icon(SNAME("Bake"), EditorStringName(EditorIcons)));
button_reset->set_icon(get_theme_icon(SNAME("Reload"), EditorStringName(EditorIcons)));
button_bake->set_button_icon(get_theme_icon(SNAME("Bake"), EditorStringName(EditorIcons)));
button_reset->set_button_icon(get_theme_icon(SNAME("Reload"), EditorStringName(EditorIcons)));
} break;
}
}
@ -65,7 +62,7 @@ void NavigationMeshEditor::_bake_pressed() {
ERR_FAIL_NULL(node);
Ref<NavigationMesh> navmesh = node->get_navigation_mesh();
if (!navmesh.is_valid()) {
if (navmesh.is_null()) {
err_dialog->set_text(TTR("A NavigationMesh resource must be set or created for this node to work."));
err_dialog->popup_centered();
return;
@ -126,14 +123,11 @@ void NavigationMeshEditor::edit(NavigationRegion3D *p_nav_region) {
node = p_nav_region;
}
void NavigationMeshEditor::_bind_methods() {
}
NavigationMeshEditor::NavigationMeshEditor() {
bake_hbox = memnew(HBoxContainer);
button_bake = memnew(Button);
button_bake->set_theme_type_variation("FlatButton");
button_bake->set_theme_type_variation(SceneStringName(FlatButton));
bake_hbox->add_child(button_bake);
button_bake->set_toggle_mode(true);
button_bake->set_text(TTR("Bake NavigationMesh"));
@ -141,7 +135,7 @@ NavigationMeshEditor::NavigationMeshEditor() {
button_bake->connect(SceneStringName(pressed), callable_mp(this, &NavigationMeshEditor::_bake_pressed));
button_reset = memnew(Button);
button_reset->set_theme_type_variation("FlatButton");
button_reset->set_theme_type_variation(SceneStringName(FlatButton));
bake_hbox->add_child(button_reset);
button_reset->set_text(TTR("Clear NavigationMesh"));
button_reset->set_tooltip_text(TTR("Clears the internal NavigationMesh vertices and polygons."));
@ -179,7 +173,7 @@ void NavigationMeshEditorPlugin::make_visible(bool p_visible) {
NavigationMeshEditorPlugin::NavigationMeshEditorPlugin() {
navigation_mesh_editor = memnew(NavigationMeshEditor);
EditorNode::get_singleton()->get_main_screen_control()->add_child(navigation_mesh_editor);
EditorNode::get_singleton()->get_gui_base()->add_child(navigation_mesh_editor);
add_control_to_container(CONTAINER_SPATIAL_EDITOR_MENU, navigation_mesh_editor->bake_hbox);
navigation_mesh_editor->hide();
navigation_mesh_editor->bake_hbox->hide();

3
engine/modules/navigation/editor/navigation_mesh_editor_plugin.h
View file

@ -60,7 +60,6 @@ class NavigationMeshEditor : public Control {
protected:
void _node_removed(Node *p_node);
static void _bind_methods();
void _notification(int p_what);
public:
@ -75,7 +74,7 @@ class NavigationMeshEditorPlugin : public EditorPlugin {
NavigationMeshEditor *navigation_mesh_editor = nullptr;
public:
virtual String get_name() const override { return "NavigationMesh"; }
virtual String get_plugin_name() const override { return "NavigationMesh"; }
bool has_main_screen() const override { return false; }
virtual void edit(Object *p_object) override;
virtual bool handles(Object *p_object) const override;

65
engine/modules/navigation/nav_agent.cpp
View file

@ -32,9 +32,6 @@
#include "nav_map.h"
NavAgent::NavAgent() {
}
void NavAgent::set_avoidance_enabled(bool p_enabled) {
avoidance_enabled = p_enabled;
_update_rvo_agent_properties();
@ -87,6 +84,8 @@ void NavAgent::_update_rvo_agent_properties() {
}
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_map(NavMap *p_map) {
@ -94,6 +93,8 @@ void NavAgent::set_map(NavMap *p_map) {
return;
}
cancel_sync_request();
if (map) {
map->remove_agent(this);
}
@ -106,6 +107,8 @@ void NavAgent::set_map(NavMap *p_map) {
if (avoidance_enabled) {
map->set_agent_as_controlled(this);
}
request_sync();
}
}
@ -156,6 +159,8 @@ void NavAgent::set_neighbor_distance(real_t p_neighbor_distance) {
rvo_agent_2d.neighborDist_ = neighbor_distance;
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_max_neighbors(int p_max_neighbors) {
@ -166,6 +171,8 @@ void NavAgent::set_max_neighbors(int p_max_neighbors) {
rvo_agent_2d.maxNeighbors_ = max_neighbors;
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_time_horizon_agents(real_t p_time_horizon) {
@ -176,6 +183,8 @@ void NavAgent::set_time_horizon_agents(real_t p_time_horizon) {
rvo_agent_2d.timeHorizon_ = time_horizon_agents;
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_time_horizon_obstacles(real_t p_time_horizon) {
@ -186,6 +195,8 @@ void NavAgent::set_time_horizon_obstacles(real_t p_time_horizon) {
rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles;
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_radius(real_t p_radius) {
@ -196,6 +207,8 @@ void NavAgent::set_radius(real_t p_radius) {
rvo_agent_2d.radius_ = radius;
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_height(real_t p_height) {
@ -206,6 +219,8 @@ void NavAgent::set_height(real_t p_height) {
rvo_agent_2d.height_ = height;
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_max_speed(real_t p_max_speed) {
@ -218,6 +233,8 @@ void NavAgent::set_max_speed(real_t p_max_speed) {
}
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_position(const Vector3 p_position) {
@ -231,6 +248,8 @@ void NavAgent::set_position(const Vector3 p_position) {
}
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_target_position(const Vector3 p_target_position) {
@ -249,6 +268,8 @@ void NavAgent::set_velocity(const Vector3 p_velocity) {
}
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_velocity_forced(const Vector3 p_velocity) {
@ -265,6 +286,8 @@ void NavAgent::set_velocity_forced(const Vector3 p_velocity) {
}
}
agent_dirty = true;
request_sync();
}
void NavAgent::update() {
@ -286,6 +309,8 @@ void NavAgent::set_avoidance_mask(uint32_t p_mask) {
rvo_agent_2d.avoidance_mask_ = avoidance_mask;
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_avoidance_layers(uint32_t p_layers) {
@ -296,6 +321,8 @@ void NavAgent::set_avoidance_layers(uint32_t p_layers) {
rvo_agent_2d.avoidance_layers_ = avoidance_layers;
}
agent_dirty = true;
request_sync();
}
void NavAgent::set_avoidance_priority(real_t p_priority) {
@ -308,12 +335,16 @@ void NavAgent::set_avoidance_priority(real_t p_priority) {
rvo_agent_2d.avoidance_priority_ = avoidance_priority;
}
agent_dirty = true;
};
bool NavAgent::check_dirty() {
const bool was_dirty = agent_dirty;
request_sync();
}
bool NavAgent::is_dirty() const {
return agent_dirty;
}
void NavAgent::sync() {
agent_dirty = false;
return was_dirty;
}
const Dictionary NavAgent::get_avoidance_data() const {
@ -372,3 +403,23 @@ void NavAgent::set_paused(bool p_paused) {
bool NavAgent::get_paused() const {
return paused;
}
void NavAgent::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() {
if (map && sync_dirty_request_list_element.in_list()) {
map->remove_agent_sync_dirty_request(&sync_dirty_request_list_element);
}
}
NavAgent::NavAgent() :
sync_dirty_request_list_element(this) {
}
NavAgent::~NavAgent() {
cancel_sync_request();
}

18
engine/modules/navigation/nav_agent.h
View file

@ -34,7 +34,7 @@
#include "nav_rid.h"
#include "core/object/class_db.h"
#include "core/templates/local_vector.h"
#include "core/templates/self_list.h"
#include <Agent2d.h>
#include <Agent3d.h>
@ -67,15 +67,18 @@ class NavAgent : public NavRid {
uint32_t avoidance_mask = 1;
real_t avoidance_priority = 1.0;
Callable avoidance_callback = Callable();
Callable avoidance_callback;
bool agent_dirty = true;
uint32_t last_map_iteration_id = 0;
bool paused = false;
SelfList<NavAgent> sync_dirty_request_list_element;
public:
NavAgent();
~NavAgent();
void set_avoidance_enabled(bool p_enabled);
bool is_avoidance_enabled() { return avoidance_enabled; }
@ -130,18 +133,21 @@ public:
const Vector3 &get_velocity_forced() const { return velocity_forced; }
void set_avoidance_layers(uint32_t p_layers);
uint32_t get_avoidance_layers() const { return avoidance_layers; };
uint32_t get_avoidance_layers() const { return avoidance_layers; }
void set_avoidance_mask(uint32_t p_mask);
uint32_t get_avoidance_mask() const { return avoidance_mask; };
uint32_t get_avoidance_mask() const { return avoidance_mask; }
void set_avoidance_priority(real_t p_priority);
real_t get_avoidance_priority() const { return avoidance_priority; };
real_t get_avoidance_priority() const { return avoidance_priority; }
void set_paused(bool p_paused);
bool get_paused() const;
bool check_dirty();
bool is_dirty() const;
void sync();
void request_sync();
void cancel_sync_request();
// Updates this agent with rvo data after the rvo simulation avoidance step.
void update();

10
engine/modules/navigation/nav_base.h
View file

@ -52,19 +52,19 @@ public:
virtual void set_use_edge_connections(bool p_enabled) {}
virtual bool get_use_edge_connections() const { return false; }
void set_navigation_layers(uint32_t p_navigation_layers) { navigation_layers = p_navigation_layers; }
virtual void set_navigation_layers(uint32_t p_navigation_layers) {}
uint32_t get_navigation_layers() const { return navigation_layers; }
void set_enter_cost(real_t p_enter_cost) { enter_cost = MAX(p_enter_cost, 0.0); }
virtual void set_enter_cost(real_t p_enter_cost) {}
real_t get_enter_cost() const { return enter_cost; }
void set_travel_cost(real_t p_travel_cost) { travel_cost = MAX(p_travel_cost, 0.0); }
virtual void set_travel_cost(real_t p_travel_cost) {}
real_t get_travel_cost() const { return travel_cost; }
void set_owner_id(ObjectID p_owner_id) { owner_id = p_owner_id; }
virtual void set_owner_id(ObjectID p_owner_id) {}
ObjectID get_owner_id() const { return owner_id; }
virtual ~NavBase(){};
virtual ~NavBase() {}
};
#endif // NAV_BASE_H

97
engine/modules/navigation/nav_link.cpp
View file

@ -37,6 +37,8 @@ void NavLink::set_map(NavMap *p_map) {
return;
}
cancel_sync_request();
if (map) {
map->remove_link(this);
}
@ -46,6 +48,7 @@ void NavLink::set_map(NavMap *p_map) {
if (map) {
map->add_link(this);
request_sync();
}
}
@ -57,7 +60,9 @@ void NavLink::set_enabled(bool p_enabled) {
// TODO: This should not require a full rebuild as the link has not really changed.
link_dirty = true;
};
request_sync();
}
void NavLink::set_bidirectional(bool p_bidirectional) {
if (bidirectional == p_bidirectional) {
@ -65,6 +70,8 @@ void NavLink::set_bidirectional(bool p_bidirectional) {
}
bidirectional = p_bidirectional;
link_dirty = true;
request_sync();
}
void NavLink::set_start_position(const Vector3 p_position) {
@ -73,6 +80,8 @@ void NavLink::set_start_position(const Vector3 p_position) {
}
start_position = p_position;
link_dirty = true;
request_sync();
}
void NavLink::set_end_position(const Vector3 p_position) {
@ -81,11 +90,91 @@ void NavLink::set_end_position(const Vector3 p_position) {
}
end_position = p_position;
link_dirty = true;
request_sync();
}
bool NavLink::check_dirty() {
const bool was_dirty = link_dirty;
void NavLink::set_navigation_layers(uint32_t p_navigation_layers) {
if (navigation_layers == p_navigation_layers) {
return;
}
navigation_layers = p_navigation_layers;
link_dirty = true;
request_sync();
}
void NavLink::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;
}
enter_cost = new_enter_cost;
link_dirty = true;
request_sync();
}
void NavLink::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;
}
travel_cost = new_travel_cost;
link_dirty = true;
request_sync();
}
void NavLink::set_owner_id(ObjectID p_owner_id) {
if (owner_id == p_owner_id) {
return;
}
owner_id = p_owner_id;
link_dirty = true;
request_sync();
}
bool NavLink::is_dirty() const {
return link_dirty;
}
void NavLink::sync() {
link_dirty = false;
return was_dirty;
}
void NavLink::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() {
if (map && sync_dirty_request_list_element.in_list()) {
map->remove_link_sync_dirty_request(&sync_dirty_request_list_element);
}
}
NavLink::NavLink() :
sync_dirty_request_list_element(this) {
type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK;
}
NavLink::~NavLink() {
cancel_sync_request();
}
void NavLink::get_iteration_update(NavLinkIteration &r_iteration) {
r_iteration.navigation_layers = get_navigation_layers();
r_iteration.enter_cost = get_enter_cost();
r_iteration.travel_cost = get_travel_cost();
r_iteration.owner_object_id = get_owner_id();
r_iteration.owner_type = get_type();
r_iteration.owner_rid = get_self();
r_iteration.enabled = get_enabled();
r_iteration.start_position = get_start_position();
r_iteration.end_position = get_end_position();
r_iteration.bidirectional = is_bidirectional();
}

34
engine/modules/navigation/nav_link.h
View file

@ -31,9 +31,23 @@
#ifndef NAV_LINK_H
#define NAV_LINK_H
#include "3d/nav_base_iteration_3d.h"
#include "nav_base.h"
#include "nav_utils.h"
struct NavLinkIteration : NavBaseIteration {
bool bidirectional = true;
Vector3 start_position;
Vector3 end_position;
LocalVector<gd::Polygon> navmesh_polygons;
Vector3 get_start_position() const { return start_position; }
Vector3 get_end_position() const { return end_position; }
bool is_bidirectional() const { return bidirectional; }
};
#include "core/templates/self_list.h"
class NavLink : public NavBase {
NavMap *map = nullptr;
bool bidirectional = true;
@ -43,10 +57,11 @@ class NavLink : public NavBase {
bool link_dirty = true;
SelfList<NavLink> sync_dirty_request_list_element;
public:
NavLink() {
type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK;
}
NavLink();
~NavLink();
void set_map(NavMap *p_map);
NavMap *get_map() const {
@ -71,7 +86,18 @@ public:
return end_position;
}
bool check_dirty();
// NavBase properties.
virtual void set_navigation_layers(uint32_t p_navigation_layers) override;
virtual void set_enter_cost(real_t p_enter_cost) override;
virtual void set_travel_cost(real_t p_travel_cost) override;
virtual void set_owner_id(ObjectID p_owner_id) override;
bool is_dirty() const;
void sync();
void request_sync();
void cancel_sync_request();
void get_iteration_update(NavLinkIteration &r_iteration);
};
#endif // NAV_LINK_H

1359
engine/modules/navigation/nav_map.cpp
View file

File diff suppressed because it is too large Load diff

103
engine/modules/navigation/nav_map.h
View file

@ -31,11 +31,14 @@
#ifndef NAV_MAP_H
#define NAV_MAP_H
#include "3d/nav_map_iteration_3d.h"
#include "3d/nav_mesh_queries_3d.h"
#include "nav_rid.h"
#include "nav_utils.h"
#include "core/math/math_defs.h"
#include "core/object/worker_thread_pool.h"
#include "servers/navigation/navigation_globals.h"
#include <KdTree2d.h>
#include <KdTree3d.h>
@ -48,41 +51,34 @@ class NavAgent;
class NavObstacle;
class NavMap : public NavRid {
RWLock map_rwlock;
/// Map Up
Vector3 up = Vector3(0, 1, 0);
/// To find the polygons edges the vertices are displaced in a grid where
/// each cell has the following cell_size and cell_height.
real_t cell_size = 0.25; // Must match ProjectSettings default 3D cell_size and NavigationMesh cell_size.
real_t cell_height = 0.25; // Must match ProjectSettings default 3D cell_height and NavigationMesh cell_height.
real_t cell_size = NavigationDefaults3D::navmesh_cell_size;
real_t cell_height = NavigationDefaults3D::navmesh_cell_height;
// For the inter-region merging to work, internal rasterization is performed.
float merge_rasterizer_cell_size = 0.25;
float merge_rasterizer_cell_height = 0.25;
Vector3 merge_rasterizer_cell_size = Vector3(cell_size, cell_height, cell_size);
// This value is used to control sensitivity of internal rasterizer.
float merge_rasterizer_cell_scale = 1.0;
bool use_edge_connections = true;
/// This value is used to detect the near edges to connect.
real_t edge_connection_margin = 0.25;
real_t edge_connection_margin = NavigationDefaults3D::edge_connection_margin;
/// This value is used to limit how far links search to find polygons to connect to.
real_t link_connection_radius = 1.0;
real_t link_connection_radius = NavigationDefaults3D::link_connection_radius;
bool regenerate_polygons = true;
bool regenerate_links = true;
bool map_settings_dirty = true;
/// Map regions
LocalVector<NavRegion *> regions;
/// Map links
LocalVector<NavLink *> links;
LocalVector<gd::Polygon> link_polygons;
/// Map polygons
LocalVector<gd::Polygon> polygons;
/// RVO avoidance worlds
RVO2D::RVOSimulator2D rvo_simulation_2d;
@ -115,14 +111,34 @@ class NavMap : public NavRid {
bool avoidance_use_high_priority_threads = true;
// Performance Monitor
int pm_region_count = 0;
int pm_agent_count = 0;
int pm_link_count = 0;
int pm_polygon_count = 0;
int pm_edge_count = 0;
int pm_edge_merge_count = 0;
int pm_edge_connection_count = 0;
int pm_edge_free_count = 0;
gd::PerformanceData performance_data;
struct {
SelfList<NavRegion>::List regions;
SelfList<NavLink>::List links;
SelfList<NavAgent>::List agents;
SelfList<NavObstacle>::List obstacles;
} sync_dirty_requests;
int path_query_slots_max = 4;
bool use_async_iterations = true;
uint32_t iteration_slot_index = 0;
LocalVector<NavMapIteration> iteration_slots;
mutable RWLock iteration_slot_rwlock;
NavMapIterationBuild 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);
bool iteration_dirty = true;
bool iteration_building = false;
bool iteration_ready = false;
void _build_iteration();
void _sync_iteration();
public:
NavMap();
@ -164,8 +180,10 @@ public:
}
gd::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);
Vector<Vector3> get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const;
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;
@ -208,22 +226,43 @@ public:
void dispatch_callbacks();
// Performance Monitor
int get_pm_region_count() const { return pm_region_count; }
int get_pm_agent_count() const { return pm_agent_count; }
int get_pm_link_count() const { return pm_link_count; }
int get_pm_polygon_count() const { return pm_polygon_count; }
int get_pm_edge_count() const { return pm_edge_count; }
int get_pm_edge_merge_count() const { return pm_edge_merge_count; }
int get_pm_edge_connection_count() const { return pm_edge_connection_count; }
int get_pm_edge_free_count() const { return pm_edge_free_count; }
int get_pm_region_count() const { return performance_data.pm_region_count; }
int get_pm_agent_count() const { return performance_data.pm_agent_count; }
int get_pm_link_count() const { return performance_data.pm_link_count; }
int get_pm_polygon_count() const { return performance_data.pm_polygon_count; }
int get_pm_edge_count() const { return performance_data.pm_edge_count; }
int get_pm_edge_merge_count() const { return performance_data.pm_edge_merge_count; }
int get_pm_edge_connection_count() const { return performance_data.pm_edge_connection_count; }
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;
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 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 set_use_async_iterations(bool p_enabled);
bool get_use_async_iterations() const;
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_avoidance_step_2d(uint32_t index, NavAgent **agent);
void compute_single_avoidance_step_3d(uint32_t index, NavAgent **agent);
void clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const;
void _sync_avoidance();
void _update_rvo_simulation();
void _update_rvo_obstacles_tree_2d();
void _update_rvo_agents_tree_2d();

50
engine/modules/navigation/nav_obstacle.cpp
View file

@ -33,10 +33,6 @@
#include "nav_agent.h"
#include "nav_map.h"
NavObstacle::NavObstacle() {}
NavObstacle::~NavObstacle() {}
void NavObstacle::set_agent(NavAgent *p_agent) {
if (agent == p_agent) {
return;
@ -45,6 +41,8 @@ void NavObstacle::set_agent(NavAgent *p_agent) {
agent = p_agent;
internal_update_agent();
request_sync();
}
void NavObstacle::set_avoidance_enabled(bool p_enabled) {
@ -56,6 +54,8 @@ void NavObstacle::set_avoidance_enabled(bool p_enabled) {
obstacle_dirty = true;
internal_update_agent();
request_sync();
}
void NavObstacle::set_use_3d_avoidance(bool p_enabled) {
@ -69,6 +69,8 @@ void NavObstacle::set_use_3d_avoidance(bool p_enabled) {
if (agent) {
agent->set_use_3d_avoidance(use_3d_avoidance);
}
request_sync();
}
void NavObstacle::set_map(NavMap *p_map) {
@ -76,6 +78,8 @@ void NavObstacle::set_map(NavMap *p_map) {
return;
}
cancel_sync_request();
if (map) {
map->remove_obstacle(this);
if (agent) {
@ -89,6 +93,8 @@ void NavObstacle::set_map(NavMap *p_map) {
if (map) {
map->add_obstacle(this);
internal_update_agent();
request_sync();
}
}
@ -103,6 +109,8 @@ void NavObstacle::set_position(const Vector3 p_position) {
if (agent) {
agent->set_position(position);
}
request_sync();
}
void NavObstacle::set_radius(real_t p_radius) {
@ -128,6 +136,8 @@ void NavObstacle::set_height(const real_t p_height) {
if (agent) {
agent->set_height(height);
}
request_sync();
}
void NavObstacle::set_velocity(const Vector3 p_velocity) {
@ -145,6 +155,8 @@ void NavObstacle::set_vertices(const Vector<Vector3> &p_vertices) {
vertices = p_vertices;
obstacle_dirty = true;
request_sync();
}
bool NavObstacle::is_map_changed() {
@ -168,12 +180,16 @@ void NavObstacle::set_avoidance_layers(uint32_t p_layers) {
if (agent) {
agent->set_avoidance_layers(avoidance_layers);
}
request_sync();
}
bool NavObstacle::check_dirty() {
const bool was_dirty = obstacle_dirty;
bool NavObstacle::is_dirty() const {
return obstacle_dirty;
}
void NavObstacle::sync() {
obstacle_dirty = false;
return was_dirty;
}
void NavObstacle::internal_update_agent() {
@ -216,3 +232,23 @@ void NavObstacle::set_paused(bool p_paused) {
bool NavObstacle::get_paused() const {
return paused;
}
void NavObstacle::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() {
if (map && sync_dirty_request_list_element.in_list()) {
map->remove_obstacle_sync_dirty_request(&sync_dirty_request_list_element);
}
}
NavObstacle::NavObstacle() :
sync_dirty_request_list_element(this) {
}
NavObstacle::~NavObstacle() {
cancel_sync_request();
}

11
engine/modules/navigation/nav_obstacle.h
View file

@ -34,7 +34,7 @@
#include "nav_rid.h"
#include "core/object/class_db.h"
#include "core/templates/local_vector.h"
#include "core/templates/self_list.h"
class NavAgent;
class NavMap;
@ -58,6 +58,8 @@ class NavObstacle : public NavRid {
uint32_t last_map_iteration_id = 0;
bool paused = false;
SelfList<NavObstacle> sync_dirty_request_list_element;
public:
NavObstacle();
~NavObstacle();
@ -92,12 +94,15 @@ public:
bool is_map_changed();
void set_avoidance_layers(uint32_t p_layers);
uint32_t get_avoidance_layers() const { return avoidance_layers; };
uint32_t get_avoidance_layers() const { return avoidance_layers; }
void set_paused(bool p_paused);
bool get_paused() const;
bool check_dirty();
bool is_dirty() const;
void sync();
void request_sync();
void cancel_sync_request();
private:
void internal_update_agent();

223
engine/modules/navigation/nav_region.cpp
View file

@ -32,11 +32,17 @@
#include "nav_map.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) {
if (map == p_map) {
return;
}
cancel_sync_request();
if (map) {
map->remove_region(this);
}
@ -44,10 +50,9 @@ void NavRegion::set_map(NavMap *p_map) {
map = p_map;
polygons_dirty = true;
connections.clear();
if (map) {
map->add_region(this);
request_sync();
}
}
@ -59,13 +64,17 @@ void NavRegion::set_enabled(bool p_enabled) {
// TODO: This should not require a full rebuild as the region has not really changed.
polygons_dirty = true;
};
request_sync();
}
void NavRegion::set_use_edge_connections(bool p_enabled) {
if (use_edge_connections != p_enabled) {
use_edge_connections = p_enabled;
polygons_dirty = true;
}
request_sync();
}
void NavRegion::set_transform(Transform3D p_transform) {
@ -75,6 +84,8 @@ void NavRegion::set_transform(Transform3D p_transform) {
transform = p_transform;
polygons_dirty = true;
request_sync();
#ifdef DEBUG_ENABLED
if (map && Math::rad_to_deg(map->get_up().angle_to(transform.basis.get_column(1))) >= 90.0f) {
ERR_PRINT_ONCE("Attempted to update a navigation region transform rotated 90 degrees or more away from the current navigation map UP orientation.");
@ -103,111 +114,81 @@ void NavRegion::set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) {
}
polygons_dirty = true;
request_sync();
}
int NavRegion::get_connections_count() const {
if (!map) {
return 0;
}
return connections.size();
Vector3 NavRegion::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);
}
Vector3 NavRegion::get_connection_pathway_start(int p_connection_id) const {
ERR_FAIL_NULL_V(map, Vector3());
ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3());
return connections[p_connection_id].pathway_start;
}
gd::ClosestPointQueryResult NavRegion::get_closest_point_info(const Vector3 &p_point) const {
RWLockRead read_lock(region_rwlock);
Vector3 NavRegion::get_connection_pathway_end(int p_connection_id) const {
ERR_FAIL_NULL_V(map, Vector3());
ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3());
return connections[p_connection_id].pathway_end;
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 {
RWLockRead read_lock(region_rwlock);
if (!get_enabled()) {
return Vector3();
}
const LocalVector<gd::Polygon> &region_polygons = get_polygons();
return NavMeshQueries3D::polygons_get_random_point(get_polygons(), p_navigation_layers, p_uniformly);
}
if (region_polygons.is_empty()) {
return Vector3();
void NavRegion::set_navigation_layers(uint32_t p_navigation_layers) {
if (navigation_layers == p_navigation_layers) {
return;
}
navigation_layers = p_navigation_layers;
region_dirty = true;
if (p_uniformly) {
real_t accumulated_area = 0;
RBMap<real_t, uint32_t> region_area_map;
request_sync();
}
for (uint32_t rp_index = 0; rp_index < region_polygons.size(); rp_index++) {
const gd::Polygon &region_polygon = region_polygons[rp_index];
real_t polyon_area = region_polygon.surface_area;
if (polyon_area == 0.0) {
continue;
}
region_area_map[accumulated_area] = rp_index;
accumulated_area += polyon_area;
}
if (region_area_map.is_empty() || accumulated_area == 0) {
// All polygons have no real surface / no area.
return Vector3();
}
real_t region_area_map_pos = Math::random(real_t(0), accumulated_area);
RBMap<real_t, uint32_t>::Iterator region_E = region_area_map.find_closest(region_area_map_pos);
ERR_FAIL_COND_V(!region_E, Vector3());
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];
real_t accumulated_polygon_area = 0;
RBMap<real_t, uint32_t> polygon_area_map;
for (uint32_t rpp_index = 2; rpp_index < rr_polygon.points.size(); rpp_index++) {
real_t face_area = Face3(rr_polygon.points[0].pos, rr_polygon.points[rpp_index - 1].pos, rr_polygon.points[rpp_index].pos).get_area();
if (face_area == 0.0) {
continue;
}
polygon_area_map[accumulated_polygon_area] = rpp_index;
accumulated_polygon_area += face_area;
}
if (polygon_area_map.is_empty() || accumulated_polygon_area == 0) {
// All faces have no real surface / no area.
return Vector3();
}
real_t polygon_area_map_pos = Math::random(real_t(0), accumulated_polygon_area);
RBMap<real_t, uint32_t>::Iterator polygon_E = polygon_area_map.find_closest(polygon_area_map_pos);
ERR_FAIL_COND_V(!polygon_E, Vector3());
uint32_t rrp_face_index = polygon_E->value;
ERR_FAIL_UNSIGNED_INDEX_V(rrp_face_index, rr_polygon.points.size(), Vector3());
const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos);
Vector3 face_random_position = face.get_random_point_inside();
return face_random_position;
} else {
uint32_t rrp_polygon_index = Math::random(int(0), region_polygons.size() - 1);
const gd::Polygon &rr_polygon = region_polygons[rrp_polygon_index];
uint32_t rrp_face_index = Math::random(int(2), rr_polygon.points.size() - 1);
const Face3 face(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos);
Vector3 face_random_position = face.get_random_point_inside();
return face_random_position;
void NavRegion::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;
}
enter_cost = new_enter_cost;
region_dirty = true;
request_sync();
}
void NavRegion::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;
}
travel_cost = new_travel_cost;
region_dirty = true;
request_sync();
}
void NavRegion::set_owner_id(ObjectID p_owner_id) {
if (owner_id == p_owner_id) {
return;
}
owner_id = p_owner_id;
region_dirty = true;
request_sync();
}
bool NavRegion::sync() {
bool something_changed = polygons_dirty /* || something_dirty? */;
RWLockWrite write_lock(region_rwlock);
bool something_changed = region_dirty || polygons_dirty;
region_dirty = false;
update_polygons();
@ -218,8 +199,9 @@ void NavRegion::update_polygons() {
if (!polygons_dirty) {
return;
}
polygons.clear();
navmesh_polygons.clear();
surface_area = 0.0;
bounds = AABB();
polygons_dirty = false;
if (map == nullptr) {
@ -239,14 +221,15 @@ void NavRegion::update_polygons() {
const Vector3 *vertices_r = pending_navmesh_vertices.ptr();
polygons.resize(pending_navmesh_polygons.size());
navmesh_polygons.resize(pending_navmesh_polygons.size());
real_t _new_region_surface_area = 0.0;
AABB _new_bounds;
// Build
bool first_vertex = true;
int navigation_mesh_polygon_index = 0;
for (gd::Polygon &polygon : polygons) {
polygon.owner = this;
for (gd::Polygon &polygon : navmesh_polygons) {
polygon.surface_area = 0.0;
Vector<int> navigation_mesh_polygon = pending_navmesh_polygons[navigation_mesh_polygon_index];
@ -286,7 +269,14 @@ void NavRegion::update_polygons() {
Vector3 point_position = transform.xform(vertices_r[idx]);
polygon.points[j].pos = point_position;
polygon.points[j].key = map->get_point_key(point_position);
polygon.points[j].key = NavMapBuilder3D::get_point_key(point_position, map->get_merge_rasterizer_cell_size());
if (first_vertex) {
first_vertex = false;
_new_bounds.position = point_position;
} else {
_new_bounds.expand_to(point_position);
}
}
if (!valid) {
@ -295,4 +285,49 @@ void NavRegion::update_polygons() {
}
surface_area = _new_region_surface_area;
bounds = _new_bounds;
}
void NavRegion::get_iteration_update(NavRegionIteration &r_iteration) {
r_iteration.navigation_layers = get_navigation_layers();
r_iteration.enter_cost = get_enter_cost();
r_iteration.travel_cost = get_travel_cost();
r_iteration.owner_object_id = get_owner_id();
r_iteration.owner_type = get_type();
r_iteration.owner_rid = get_self();
r_iteration.enabled = get_enabled();
r_iteration.transform = get_transform();
r_iteration.owner_use_edge_connections = get_use_edge_connections();
r_iteration.bounds = get_bounds();
r_iteration.surface_area = get_surface_area();
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];
navmesh_polygon.owner = &r_iteration;
r_iteration.navmesh_polygons[i] = navmesh_polygon;
}
}
void NavRegion::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() {
if (map && sync_dirty_request_list_element.in_list()) {
map->remove_region_sync_dirty_request(&sync_dirty_request_list_element);
}
}
NavRegion::NavRegion() :
sync_dirty_request_list_element(this) {
type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION;
}
NavRegion::~NavRegion() {
cancel_sync_request();
}

47
engine/modules/navigation/nav_region.h
View file

@ -37,29 +37,34 @@
#include "core/os/rw_lock.h"
#include "scene/resources/navigation_mesh.h"
struct NavRegionIteration;
class NavRegion : public NavBase {
RWLock region_rwlock;
NavMap *map = nullptr;
Transform3D transform;
Vector<gd::Edge::Connection> connections;
bool enabled = true;
bool use_edge_connections = true;
bool region_dirty = true;
bool polygons_dirty = true;
/// Cache
LocalVector<gd::Polygon> polygons;
LocalVector<gd::Polygon> navmesh_polygons;
real_t surface_area = 0.0;
AABB bounds;
RWLock navmesh_rwlock;
Vector<Vector3> pending_navmesh_vertices;
Vector<Vector<int>> pending_navmesh_polygons;
SelfList<NavRegion> sync_dirty_request_list_element;
public:
NavRegion() {
type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION;
}
NavRegion();
~NavRegion();
void scratch_polygons() {
polygons_dirty = true;
@ -73,10 +78,8 @@ public:
return map;
}
void set_use_edge_connections(bool p_enabled);
bool get_use_edge_connections() const {
return use_edge_connections;
}
virtual void set_use_edge_connections(bool p_enabled) override;
virtual bool get_use_edge_connections() const override { return use_edge_connections; }
void set_transform(Transform3D transform);
const Transform3D &get_transform() const {
@ -85,22 +88,28 @@ public:
void set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh);
Vector<gd::Edge::Connection> &get_connections() {
return connections;
}
int get_connections_count() const;
Vector3 get_connection_pathway_start(int p_connection_id) const;
Vector3 get_connection_pathway_end(int p_connection_id) const;
LocalVector<gd::Polygon> const &get_polygons() const {
return polygons;
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;
Vector3 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const;
real_t get_surface_area() const { return surface_area; };
real_t get_surface_area() const { return surface_area; }
AABB get_bounds() const { return bounds; }
// NavBase properties.
virtual void set_navigation_layers(uint32_t p_navigation_layers) override;
virtual void set_enter_cost(real_t p_enter_cost) override;
virtual void set_travel_cost(real_t p_travel_cost) override;
virtual void set_owner_id(ObjectID p_owner_id) override;
bool sync();
void request_sync();
void cancel_sync_request();
void get_iteration_update(NavRegionIteration &r_iteration);
private:
void update_polygons();

223
engine/modules/navigation/nav_utils.h
View file

@ -35,8 +35,9 @@
#include "core/templates/hash_map.h"
#include "core/templates/hashfuncs.h"
#include "core/templates/local_vector.h"
#include "servers/navigation/navigation_utilities.h"
class NavBase;
struct NavBaseIteration;
namespace gd {
struct Polygon;
@ -94,12 +95,15 @@ struct Edge {
};
/// Connections from this edge to other polygons.
Vector<Connection> connections;
LocalVector<Connection> connections;
};
struct Polygon {
/// Id of the polygon in the map.
uint32_t id = UINT32_MAX;
/// Navigation region or link that contains this polygon.
const NavBase *owner = nullptr;
const NavBaseIteration *owner = nullptr;
/// The points of this `Polygon`
LocalVector<Point> points;
@ -111,9 +115,11 @@ struct Polygon {
};
struct NavigationPoly {
uint32_t self_id = 0;
/// This poly.
const Polygon *poly;
const Polygon *poly = nullptr;
/// Index in the heap of traversable polygons.
uint32_t traversable_poly_index = UINT32_MAX;
/// Those 4 variables are used to travel the path backwards.
int back_navigation_poly_id = -1;
@ -123,20 +129,53 @@ struct NavigationPoly {
/// The entry position of this poly.
Vector3 entry;
/// The distance to the destination.
/// The distance traveled until now (g cost).
real_t traveled_distance = 0.0;
/// The distance to the destination (h cost).
real_t distance_to_destination = 0.0;
NavigationPoly() { poly = nullptr; }
NavigationPoly(const Polygon *p_poly) :
poly(p_poly) {}
bool operator==(const NavigationPoly &other) const {
return poly == other.poly;
/// The total travel cost (f cost).
real_t total_travel_cost() const {
return traveled_distance + distance_to_destination;
}
bool operator!=(const NavigationPoly &other) const {
return !operator==(other);
bool operator==(const NavigationPoly &p_other) const {
return poly == p_other.poly;
}
bool operator!=(const NavigationPoly &p_other) const {
return !(*this == p_other);
}
void reset() {
poly = nullptr;
traversable_poly_index = UINT32_MAX;
back_navigation_poly_id = -1;
back_navigation_edge = -1;
traveled_distance = FLT_MAX;
distance_to_destination = 0.0;
}
};
struct NavPolyTravelCostGreaterThan {
// Returns `true` if the travel cost of `a` is higher than that of `b`.
bool operator()(const NavigationPoly *p_poly_a, const NavigationPoly *p_poly_b) const {
real_t f_cost_a = p_poly_a->total_travel_cost();
real_t h_cost_a = p_poly_a->distance_to_destination;
real_t f_cost_b = p_poly_b->total_travel_cost();
real_t h_cost_b = p_poly_b->distance_to_destination;
if (f_cost_a != f_cost_b) {
return f_cost_a > f_cost_b;
} else {
return h_cost_a > h_cost_b;
}
}
};
struct NavPolyHeapIndexer {
void operator()(NavigationPoly *p_poly, uint32_t p_heap_index) const {
p_poly->traversable_poly_index = p_heap_index;
}
};
@ -146,6 +185,160 @@ 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];
int size = 0;
};
struct PerformanceData {
int pm_region_count = 0;
int pm_agent_count = 0;
int pm_link_count = 0;
int pm_polygon_count = 0;
int pm_edge_count = 0;
int pm_edge_merge_count = 0;
int pm_edge_connection_count = 0;
int pm_edge_free_count = 0;
int pm_obstacle_count = 0;
void reset() {
pm_region_count = 0;
pm_agent_count = 0;
pm_link_count = 0;
pm_polygon_count = 0;
pm_edge_count = 0;
pm_edge_merge_count = 0;
pm_edge_connection_count = 0;
pm_edge_free_count = 0;
pm_obstacle_count = 0;
}
};
} // namespace gd
#endif // NAV_UTILS_H