generated from hertog/godot-module-template
Initial commit
This commit is contained in:
commit
65227bf3a5
12416 changed files with 6001067 additions and 0 deletions
550
engine/modules/navigation/2d/godot_navigation_server_2d.cpp
Normal file
550
engine/modules/navigation/2d/godot_navigation_server_2d.cpp
Normal file
|
|
@ -0,0 +1,550 @@
|
|||
/**************************************************************************/
|
||||
/* godot_navigation_server_2d.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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#include "godot_navigation_server_2d.h"
|
||||
|
||||
#ifdef CLIPPER2_ENABLED
|
||||
#include "nav_mesh_generator_2d.h"
|
||||
#endif // CLIPPER2_ENABLED
|
||||
|
||||
#include "servers/navigation_server_3d.h"
|
||||
|
||||
#define FORWARD_0(FUNC_NAME) \
|
||||
GodotNavigationServer2D::FUNC_NAME() { \
|
||||
return NavigationServer3D::get_singleton()->FUNC_NAME(); \
|
||||
}
|
||||
|
||||
#define FORWARD_0_C(FUNC_NAME) \
|
||||
GodotNavigationServer2D::FUNC_NAME() \
|
||||
const { \
|
||||
return NavigationServer3D::get_singleton()->FUNC_NAME(); \
|
||||
}
|
||||
|
||||
#define FORWARD_1(FUNC_NAME, T_0, D_0, CONV_0) \
|
||||
GodotNavigationServer2D::FUNC_NAME(T_0 D_0) { \
|
||||
return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0)); \
|
||||
}
|
||||
|
||||
#define FORWARD_1_C(FUNC_NAME, T_0, D_0, CONV_0) \
|
||||
GodotNavigationServer2D::FUNC_NAME(T_0 D_0) \
|
||||
const { \
|
||||
return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0)); \
|
||||
}
|
||||
|
||||
#define FORWARD_1_R_C(CONV_R, FUNC_NAME, T_0, D_0, CONV_0) \
|
||||
GodotNavigationServer2D::FUNC_NAME(T_0 D_0) \
|
||||
const { \
|
||||
return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0))); \
|
||||
}
|
||||
|
||||
#define FORWARD_2(FUNC_NAME, T_0, D_0, T_1, D_1, CONV_0, CONV_1) \
|
||||
GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1) { \
|
||||
return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1)); \
|
||||
}
|
||||
|
||||
#define FORWARD_2_C(FUNC_NAME, T_0, D_0, T_1, D_1, CONV_0, CONV_1) \
|
||||
GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1) \
|
||||
const { \
|
||||
return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1)); \
|
||||
}
|
||||
|
||||
#define FORWARD_2_R_C(CONV_R, FUNC_NAME, T_0, D_0, T_1, D_1, CONV_0, CONV_1) \
|
||||
GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1) \
|
||||
const { \
|
||||
return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1))); \
|
||||
}
|
||||
|
||||
static RID rid_to_rid(const RID d) {
|
||||
return d;
|
||||
}
|
||||
|
||||
static bool bool_to_bool(const bool d) {
|
||||
return d;
|
||||
}
|
||||
|
||||
static int int_to_int(const int d) {
|
||||
return d;
|
||||
}
|
||||
|
||||
static uint32_t uint32_to_uint32(const uint32_t d) {
|
||||
return d;
|
||||
}
|
||||
|
||||
static real_t real_to_real(const real_t d) {
|
||||
return d;
|
||||
}
|
||||
|
||||
static Vector3 v2_to_v3(const Vector2 d) {
|
||||
return Vector3(d.x, 0.0, d.y);
|
||||
}
|
||||
|
||||
static Vector2 v3_to_v2(const Vector3 &d) {
|
||||
return Vector2(d.x, d.z);
|
||||
}
|
||||
|
||||
static Vector<Vector3> vector_v2_to_v3(const Vector<Vector2> &d) {
|
||||
Vector<Vector3> nd;
|
||||
nd.resize(d.size());
|
||||
for (int i(0); i < nd.size(); i++) {
|
||||
nd.write[i] = v2_to_v3(d[i]);
|
||||
}
|
||||
return nd;
|
||||
}
|
||||
|
||||
static Vector<Vector2> vector_v3_to_v2(const Vector<Vector3> &d) {
|
||||
Vector<Vector2> nd;
|
||||
nd.resize(d.size());
|
||||
for (int i(0); i < nd.size(); i++) {
|
||||
nd.write[i] = v3_to_v2(d[i]);
|
||||
}
|
||||
return nd;
|
||||
}
|
||||
|
||||
static Transform3D trf2_to_trf3(const Transform2D &d) {
|
||||
Vector3 o(v2_to_v3(d.get_origin()));
|
||||
Basis b;
|
||||
b.rotate(Vector3(0, -1, 0), d.get_rotation());
|
||||
b.scale(v2_to_v3(d.get_scale()));
|
||||
return Transform3D(b, o);
|
||||
}
|
||||
|
||||
static Transform2D trf3_to_trf2(const Transform3D &d) {
|
||||
Vector3 o = d.get_origin();
|
||||
Vector3 nx = d.xform(Vector3(1, 0, 0)) - o;
|
||||
Vector3 nz = d.xform(Vector3(0, 0, 1)) - o;
|
||||
return Transform2D(nx.x, nx.z, nz.x, nz.z, o.x, o.z);
|
||||
}
|
||||
|
||||
static ObjectID id_to_id(const ObjectID &id) {
|
||||
return id;
|
||||
}
|
||||
|
||||
static Callable callable_to_callable(const Callable &c) {
|
||||
return c;
|
||||
}
|
||||
|
||||
static Ref<NavigationMesh> poly_to_mesh(Ref<NavigationPolygon> d) {
|
||||
if (d.is_valid()) {
|
||||
return d->get_navigation_mesh();
|
||||
} else {
|
||||
return Ref<NavigationMesh>();
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
void GodotNavigationServer2D::sync() {
|
||||
#ifdef CLIPPER2_ENABLED
|
||||
if (navmesh_generator_2d) {
|
||||
navmesh_generator_2d->sync();
|
||||
}
|
||||
#endif // CLIPPER2_ENABLED
|
||||
}
|
||||
|
||||
void GodotNavigationServer2D::finish() {
|
||||
#ifdef CLIPPER2_ENABLED
|
||||
if (navmesh_generator_2d) {
|
||||
navmesh_generator_2d->finish();
|
||||
memdelete(navmesh_generator_2d);
|
||||
navmesh_generator_2d = nullptr;
|
||||
}
|
||||
#endif // CLIPPER2_ENABLED
|
||||
}
|
||||
|
||||
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_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.");
|
||||
|
||||
#ifdef CLIPPER2_ENABLED
|
||||
ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
|
||||
NavMeshGenerator2D::get_singleton()->parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node, p_callback);
|
||||
#endif // CLIPPER2_ENABLED
|
||||
}
|
||||
|
||||
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_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());
|
||||
NavMeshGenerator2D::get_singleton()->bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
|
||||
#endif // CLIPPER2_ENABLED
|
||||
}
|
||||
|
||||
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_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());
|
||||
NavMeshGenerator2D::get_singleton()->bake_from_source_geometry_data_async(p_navigation_mesh, p_source_geometry_data, p_callback);
|
||||
#endif // CLIPPER2_ENABLED
|
||||
}
|
||||
|
||||
bool GodotNavigationServer2D::is_baking_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon) const {
|
||||
#ifdef CLIPPER2_ENABLED
|
||||
return NavMeshGenerator2D::get_singleton()->is_baking(p_navigation_polygon);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
|
||||
Vector<Vector2> GodotNavigationServer2D::simplify_path(const Vector<Vector2> &p_path, real_t p_epsilon) {
|
||||
return vector_v3_to_v2(NavigationServer3D::get_singleton()->simplify_path(vector_v2_to_v3(p_path), p_epsilon));
|
||||
}
|
||||
|
||||
GodotNavigationServer2D::GodotNavigationServer2D() {}
|
||||
|
||||
GodotNavigationServer2D::~GodotNavigationServer2D() {}
|
||||
|
||||
TypedArray<RID> FORWARD_0_C(get_maps);
|
||||
|
||||
TypedArray<RID> FORWARD_1_C(map_get_links, RID, p_map, rid_to_rid);
|
||||
|
||||
TypedArray<RID> FORWARD_1_C(map_get_regions, RID, p_map, rid_to_rid);
|
||||
|
||||
TypedArray<RID> FORWARD_1_C(map_get_agents, RID, p_map, rid_to_rid);
|
||||
|
||||
TypedArray<RID> FORWARD_1_C(map_get_obstacles, RID, p_map, rid_to_rid);
|
||||
|
||||
RID FORWARD_1_C(region_get_map, RID, p_region, rid_to_rid);
|
||||
|
||||
RID FORWARD_1_C(agent_get_map, RID, p_agent, rid_to_rid);
|
||||
|
||||
RID FORWARD_0(map_create);
|
||||
|
||||
void FORWARD_2(map_set_active, RID, p_map, bool, p_active, rid_to_rid, bool_to_bool);
|
||||
|
||||
bool FORWARD_1_C(map_is_active, RID, p_map, rid_to_rid);
|
||||
|
||||
void GodotNavigationServer2D::map_force_update(RID p_map) {
|
||||
NavigationServer3D::get_singleton()->map_force_update(p_map);
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
void FORWARD_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled, rid_to_rid, bool_to_bool);
|
||||
bool FORWARD_1_C(map_get_use_edge_connections, RID, p_map, rid_to_rid);
|
||||
|
||||
void FORWARD_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin, rid_to_rid, real_to_real);
|
||||
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> 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);
|
||||
|
||||
Vector2 GodotNavigationServer2D::map_get_random_point(RID p_map, uint32_t p_naviation_layers, bool p_uniformly) const {
|
||||
Vector3 result = NavigationServer3D::get_singleton()->map_get_random_point(p_map, p_naviation_layers, p_uniformly);
|
||||
return v3_to_v2(result);
|
||||
}
|
||||
|
||||
RID FORWARD_0(region_create);
|
||||
void FORWARD_2(region_set_enabled, RID, p_region, bool, p_enabled, rid_to_rid, bool_to_bool);
|
||||
bool FORWARD_1_C(region_get_enabled, RID, p_region, rid_to_rid);
|
||||
void FORWARD_2(region_set_use_edge_connections, RID, p_region, bool, p_enabled, rid_to_rid, bool_to_bool);
|
||||
bool FORWARD_1_C(region_get_use_edge_connections, RID, p_region, rid_to_rid);
|
||||
|
||||
void FORWARD_2(region_set_enter_cost, RID, p_region, real_t, p_enter_cost, rid_to_rid, real_to_real);
|
||||
real_t FORWARD_1_C(region_get_enter_cost, RID, p_region, rid_to_rid);
|
||||
void FORWARD_2(region_set_travel_cost, RID, p_region, real_t, p_travel_cost, rid_to_rid, real_to_real);
|
||||
real_t FORWARD_1_C(region_get_travel_cost, RID, p_region, rid_to_rid);
|
||||
void FORWARD_2(region_set_owner_id, RID, p_region, ObjectID, p_owner_id, rid_to_rid, id_to_id);
|
||||
ObjectID FORWARD_1_C(region_get_owner_id, RID, p_region, rid_to_rid);
|
||||
bool FORWARD_2_C(region_owns_point, RID, p_region, const Vector2 &, p_point, rid_to_rid, v2_to_v3);
|
||||
|
||||
void FORWARD_2(region_set_map, RID, p_region, RID, p_map, rid_to_rid, rid_to_rid);
|
||||
void FORWARD_2(region_set_navigation_layers, RID, p_region, uint32_t, p_navigation_layers, rid_to_rid, uint32_to_uint32);
|
||||
uint32_t FORWARD_1_C(region_get_navigation_layers, RID, p_region, rid_to_rid);
|
||||
void FORWARD_2(region_set_transform, RID, p_region, Transform2D, p_transform, rid_to_rid, trf2_to_trf3);
|
||||
|
||||
Transform2D GodotNavigationServer2D::region_get_transform(RID p_region) const {
|
||||
return trf3_to_trf2(NavigationServer3D::get_singleton()->region_get_transform(p_region));
|
||||
}
|
||||
|
||||
void GodotNavigationServer2D::region_set_navigation_polygon(RID p_region, Ref<NavigationPolygon> p_navigation_polygon) {
|
||||
NavigationServer3D::get_singleton()->region_set_navigation_mesh(p_region, poly_to_mesh(p_navigation_polygon));
|
||||
}
|
||||
|
||||
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);
|
||||
RID FORWARD_1_C(link_get_map, RID, p_link, rid_to_rid);
|
||||
void FORWARD_2(link_set_enabled, RID, p_link, bool, p_enabled, rid_to_rid, bool_to_bool);
|
||||
bool FORWARD_1_C(link_get_enabled, RID, p_link, rid_to_rid);
|
||||
void FORWARD_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional, rid_to_rid, bool_to_bool);
|
||||
bool FORWARD_1_C(link_is_bidirectional, RID, p_link, rid_to_rid);
|
||||
void FORWARD_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers, rid_to_rid, uint32_to_uint32);
|
||||
uint32_t FORWARD_1_C(link_get_navigation_layers, RID, p_link, rid_to_rid);
|
||||
void FORWARD_2(link_set_start_position, RID, p_link, Vector2, p_position, rid_to_rid, v2_to_v3);
|
||||
Vector2 FORWARD_1_R_C(v3_to_v2, link_get_start_position, RID, p_link, rid_to_rid);
|
||||
void FORWARD_2(link_set_end_position, RID, p_link, Vector2, p_position, rid_to_rid, v2_to_v3);
|
||||
Vector2 FORWARD_1_R_C(v3_to_v2, link_get_end_position, RID, p_link, rid_to_rid);
|
||||
void FORWARD_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost, rid_to_rid, real_to_real);
|
||||
real_t FORWARD_1_C(link_get_enter_cost, RID, p_link, rid_to_rid);
|
||||
void FORWARD_2(link_set_travel_cost, RID, p_link, real_t, p_travel_cost, rid_to_rid, real_to_real);
|
||||
real_t FORWARD_1_C(link_get_travel_cost, RID, p_link, rid_to_rid);
|
||||
void FORWARD_2(link_set_owner_id, RID, p_link, ObjectID, p_owner_id, rid_to_rid, id_to_id);
|
||||
ObjectID FORWARD_1_C(link_get_owner_id, RID, p_link, rid_to_rid);
|
||||
|
||||
RID GodotNavigationServer2D::agent_create() {
|
||||
RID agent = NavigationServer3D::get_singleton()->agent_create();
|
||||
return agent;
|
||||
}
|
||||
|
||||
void FORWARD_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled, rid_to_rid, bool_to_bool);
|
||||
bool FORWARD_1_C(agent_get_avoidance_enabled, RID, p_agent, rid_to_rid);
|
||||
void FORWARD_2(agent_set_map, RID, p_agent, RID, p_map, rid_to_rid, rid_to_rid);
|
||||
void FORWARD_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_dist, rid_to_rid, real_to_real);
|
||||
real_t GodotNavigationServer2D::agent_get_neighbor_distance(RID p_agent) const {
|
||||
return NavigationServer3D::get_singleton()->agent_get_neighbor_distance(p_agent);
|
||||
}
|
||||
void FORWARD_2(agent_set_max_neighbors, RID, p_agent, int, p_count, rid_to_rid, int_to_int);
|
||||
int GodotNavigationServer2D::agent_get_max_neighbors(RID p_agent) const {
|
||||
return NavigationServer3D::get_singleton()->agent_get_max_neighbors(p_agent);
|
||||
}
|
||||
void FORWARD_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon, rid_to_rid, real_to_real);
|
||||
real_t GodotNavigationServer2D::agent_get_time_horizon_agents(RID p_agent) const {
|
||||
return NavigationServer3D::get_singleton()->agent_get_time_horizon_agents(p_agent);
|
||||
}
|
||||
void FORWARD_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon, rid_to_rid, real_to_real);
|
||||
real_t GodotNavigationServer2D::agent_get_time_horizon_obstacles(RID p_agent) const {
|
||||
return NavigationServer3D::get_singleton()->agent_get_time_horizon_obstacles(p_agent);
|
||||
}
|
||||
void FORWARD_2(agent_set_radius, RID, p_agent, real_t, p_radius, rid_to_rid, real_to_real);
|
||||
real_t GodotNavigationServer2D::agent_get_radius(RID p_agent) const {
|
||||
return NavigationServer3D::get_singleton()->agent_get_radius(p_agent);
|
||||
}
|
||||
void FORWARD_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed, rid_to_rid, real_to_real);
|
||||
real_t GodotNavigationServer2D::agent_get_max_speed(RID p_agent) const {
|
||||
return NavigationServer3D::get_singleton()->agent_get_max_speed(p_agent);
|
||||
}
|
||||
void FORWARD_2(agent_set_velocity_forced, RID, p_agent, Vector2, p_velocity, rid_to_rid, v2_to_v3);
|
||||
void FORWARD_2(agent_set_velocity, RID, p_agent, Vector2, p_velocity, rid_to_rid, v2_to_v3);
|
||||
Vector2 GodotNavigationServer2D::agent_get_velocity(RID p_agent) const {
|
||||
return v3_to_v2(NavigationServer3D::get_singleton()->agent_get_velocity(p_agent));
|
||||
}
|
||||
void FORWARD_2(agent_set_position, RID, p_agent, Vector2, p_position, rid_to_rid, v2_to_v3);
|
||||
Vector2 GodotNavigationServer2D::agent_get_position(RID p_agent) const {
|
||||
return v3_to_v2(NavigationServer3D::get_singleton()->agent_get_position(p_agent));
|
||||
}
|
||||
bool FORWARD_1_C(agent_is_map_changed, RID, p_agent, rid_to_rid);
|
||||
void FORWARD_2(agent_set_paused, RID, p_agent, bool, p_paused, rid_to_rid, bool_to_bool);
|
||||
bool FORWARD_1_C(agent_get_paused, RID, p_agent, rid_to_rid);
|
||||
|
||||
void GodotNavigationServer2D::free(RID 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;
|
||||
}
|
||||
NavigationServer3D::get_singleton()->free(p_object);
|
||||
}
|
||||
|
||||
void FORWARD_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback, rid_to_rid, callable_to_callable);
|
||||
bool GodotNavigationServer2D::agent_has_avoidance_callback(RID p_agent) const {
|
||||
return NavigationServer3D::get_singleton()->agent_has_avoidance_callback(p_agent);
|
||||
}
|
||||
|
||||
void FORWARD_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers, rid_to_rid, uint32_to_uint32);
|
||||
uint32_t GodotNavigationServer2D::agent_get_avoidance_layers(RID p_agent) const {
|
||||
return NavigationServer3D::get_singleton()->agent_get_avoidance_layers(p_agent);
|
||||
}
|
||||
void FORWARD_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask, rid_to_rid, uint32_to_uint32);
|
||||
uint32_t GodotNavigationServer2D::agent_get_avoidance_mask(RID p_agent) const {
|
||||
return NavigationServer3D::get_singleton()->agent_get_avoidance_mask(p_agent);
|
||||
}
|
||||
void FORWARD_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority, rid_to_rid, real_to_real);
|
||||
real_t GodotNavigationServer2D::agent_get_avoidance_priority(RID p_agent) const {
|
||||
return NavigationServer3D::get_singleton()->agent_get_avoidance_priority(p_agent);
|
||||
}
|
||||
|
||||
RID GodotNavigationServer2D::obstacle_create() {
|
||||
RID obstacle = NavigationServer3D::get_singleton()->obstacle_create();
|
||||
return obstacle;
|
||||
}
|
||||
void FORWARD_2(obstacle_set_avoidance_enabled, RID, p_obstacle, bool, p_enabled, rid_to_rid, bool_to_bool);
|
||||
bool FORWARD_1_C(obstacle_get_avoidance_enabled, RID, p_obstacle, rid_to_rid);
|
||||
void FORWARD_2(obstacle_set_map, RID, p_obstacle, RID, p_map, rid_to_rid, rid_to_rid);
|
||||
RID FORWARD_1_C(obstacle_get_map, RID, p_obstacle, rid_to_rid);
|
||||
void FORWARD_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused, rid_to_rid, bool_to_bool);
|
||||
bool FORWARD_1_C(obstacle_get_paused, RID, p_obstacle, rid_to_rid);
|
||||
void FORWARD_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius, rid_to_rid, real_to_real);
|
||||
real_t GodotNavigationServer2D::obstacle_get_radius(RID p_obstacle) const {
|
||||
return NavigationServer3D::get_singleton()->obstacle_get_radius(p_obstacle);
|
||||
}
|
||||
void FORWARD_2(obstacle_set_velocity, RID, p_obstacle, Vector2, p_velocity, rid_to_rid, v2_to_v3);
|
||||
Vector2 GodotNavigationServer2D::obstacle_get_velocity(RID p_obstacle) const {
|
||||
return v3_to_v2(NavigationServer3D::get_singleton()->obstacle_get_velocity(p_obstacle));
|
||||
}
|
||||
void FORWARD_2(obstacle_set_position, RID, p_obstacle, Vector2, p_position, rid_to_rid, v2_to_v3);
|
||||
Vector2 GodotNavigationServer2D::obstacle_get_position(RID p_obstacle) const {
|
||||
return v3_to_v2(NavigationServer3D::get_singleton()->obstacle_get_position(p_obstacle));
|
||||
}
|
||||
void FORWARD_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers, rid_to_rid, uint32_to_uint32);
|
||||
uint32_t GodotNavigationServer2D::obstacle_get_avoidance_layers(RID p_obstacle) const {
|
||||
return NavigationServer3D::get_singleton()->obstacle_get_avoidance_layers(p_obstacle);
|
||||
}
|
||||
|
||||
void GodotNavigationServer2D::obstacle_set_vertices(RID p_obstacle, const Vector<Vector2> &p_vertices) {
|
||||
NavigationServer3D::get_singleton()->obstacle_set_vertices(p_obstacle, vector_v2_to_v3(p_vertices));
|
||||
}
|
||||
Vector<Vector2> GodotNavigationServer2D::obstacle_get_vertices(RID p_obstacle) const {
|
||||
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 Callable &p_callback) {
|
||||
ERR_FAIL_COND(p_query_parameters.is_null());
|
||||
ERR_FAIL_COND(p_query_result.is_null());
|
||||
|
||||
Ref<NavigationPathQueryParameters3D> query_parameters;
|
||||
query_parameters.instantiate();
|
||||
|
||||
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
|
||||
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) {
|
||||
RWLockWrite write_lock(geometry_parser_rwlock);
|
||||
|
||||
NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_parser);
|
||||
ERR_FAIL_NULL(parser);
|
||||
|
||||
parser->callback = p_callback;
|
||||
}
|
||||
266
engine/modules/navigation/2d/godot_navigation_server_2d.h
Normal file
266
engine/modules/navigation/2d/godot_navigation_server_2d.h
Normal file
|
|
@ -0,0 +1,266 @@
|
|||
/**************************************************************************/
|
||||
/* godot_navigation_server_2d.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 GODOT_NAVIGATION_SERVER_2D_H
|
||||
#define GODOT_NAVIGATION_SERVER_2D_H
|
||||
|
||||
#include "../nav_agent.h"
|
||||
#include "../nav_link.h"
|
||||
#include "../nav_map.h"
|
||||
#include "../nav_obstacle.h"
|
||||
#include "../nav_region.h"
|
||||
|
||||
#include "servers/navigation_server_2d.h"
|
||||
|
||||
#ifdef CLIPPER2_ENABLED
|
||||
class NavMeshGenerator2D;
|
||||
#endif // CLIPPER2_ENABLED
|
||||
|
||||
// This server exposes the `NavigationServer3D` features in the 2D world.
|
||||
class GodotNavigationServer2D : public NavigationServer2D {
|
||||
GDCLASS(GodotNavigationServer2D, NavigationServer2D);
|
||||
|
||||
#ifdef CLIPPER2_ENABLED
|
||||
NavMeshGenerator2D *navmesh_generator_2d = nullptr;
|
||||
#endif // CLIPPER2_ENABLED
|
||||
|
||||
public:
|
||||
GodotNavigationServer2D();
|
||||
virtual ~GodotNavigationServer2D();
|
||||
|
||||
virtual TypedArray<RID> get_maps() const override;
|
||||
|
||||
virtual RID map_create() override;
|
||||
virtual void map_set_active(RID p_map, bool p_active) override;
|
||||
virtual bool map_is_active(RID p_map) const override;
|
||||
virtual void map_set_cell_size(RID p_map, real_t p_cell_size) override;
|
||||
virtual real_t map_get_cell_size(RID p_map) const override;
|
||||
virtual void map_set_use_edge_connections(RID p_map, bool p_enabled) override;
|
||||
virtual bool map_get_use_edge_connections(RID p_map) const override;
|
||||
virtual void map_set_edge_connection_margin(RID p_map, real_t p_connection_margin) override;
|
||||
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) 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;
|
||||
virtual TypedArray<RID> map_get_regions(RID p_map) const override;
|
||||
virtual TypedArray<RID> map_get_agents(RID p_map) const override;
|
||||
virtual TypedArray<RID> map_get_obstacles(RID p_map) const override;
|
||||
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;
|
||||
virtual bool region_get_enabled(RID p_region) const override;
|
||||
virtual void region_set_use_edge_connections(RID p_region, bool p_enabled) override;
|
||||
virtual bool region_get_use_edge_connections(RID p_region) const override;
|
||||
virtual void region_set_enter_cost(RID p_region, real_t p_enter_cost) override;
|
||||
virtual real_t region_get_enter_cost(RID p_region) const override;
|
||||
virtual void region_set_travel_cost(RID p_region, real_t p_travel_cost) override;
|
||||
virtual real_t region_get_travel_cost(RID p_region) const override;
|
||||
virtual void region_set_owner_id(RID p_region, ObjectID p_owner_id) override;
|
||||
virtual ObjectID region_get_owner_id(RID p_region) const override;
|
||||
virtual bool region_owns_point(RID p_region, const Vector2 &p_point) const override;
|
||||
virtual void region_set_map(RID p_region, RID p_map) override;
|
||||
virtual RID region_get_map(RID p_region) const override;
|
||||
virtual void region_set_navigation_layers(RID p_region, uint32_t p_navigation_layers) override;
|
||||
virtual uint32_t region_get_navigation_layers(RID p_region) const override;
|
||||
virtual void region_set_transform(RID p_region, Transform2D p_transform) override;
|
||||
virtual Transform2D region_get_transform(RID p_region) const override;
|
||||
virtual void region_set_navigation_polygon(RID p_region, Ref<NavigationPolygon> p_navigation_polygon) override;
|
||||
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;
|
||||
|
||||
/// Set the map of this link.
|
||||
virtual void link_set_map(RID p_link, RID p_map) override;
|
||||
virtual RID link_get_map(RID p_link) const override;
|
||||
|
||||
virtual void link_set_enabled(RID p_link, bool p_enabled) override;
|
||||
virtual bool link_get_enabled(RID p_link) const override;
|
||||
|
||||
/// Set whether this link travels in both directions.
|
||||
virtual void link_set_bidirectional(RID p_link, bool p_bidirectional) override;
|
||||
virtual bool link_is_bidirectional(RID p_link) const override;
|
||||
|
||||
/// Set the link's layers.
|
||||
virtual void link_set_navigation_layers(RID p_link, uint32_t p_navigation_layers) override;
|
||||
virtual uint32_t link_get_navigation_layers(RID p_link) const override;
|
||||
|
||||
/// Set the start position of the link.
|
||||
virtual void link_set_start_position(RID p_link, Vector2 p_position) override;
|
||||
virtual Vector2 link_get_start_position(RID p_link) const override;
|
||||
|
||||
/// Set the end position of the link.
|
||||
virtual void link_set_end_position(RID p_link, Vector2 p_position) override;
|
||||
virtual Vector2 link_get_end_position(RID p_link) const override;
|
||||
|
||||
/// Set the enter cost of the link.
|
||||
virtual void link_set_enter_cost(RID p_link, real_t p_enter_cost) override;
|
||||
virtual real_t link_get_enter_cost(RID p_link) const override;
|
||||
|
||||
/// Set the travel cost of the link.
|
||||
virtual void link_set_travel_cost(RID p_link, real_t p_travel_cost) override;
|
||||
virtual real_t link_get_travel_cost(RID p_link) const override;
|
||||
|
||||
/// Set the node which manages this link.
|
||||
virtual void link_set_owner_id(RID p_link, ObjectID p_owner_id) override;
|
||||
virtual ObjectID link_get_owner_id(RID p_link) const override;
|
||||
|
||||
/// Creates the agent.
|
||||
virtual RID agent_create() override;
|
||||
|
||||
/// Put the agent in the map.
|
||||
virtual void agent_set_map(RID p_agent, RID p_map) override;
|
||||
virtual RID agent_get_map(RID p_agent) const override;
|
||||
|
||||
virtual void agent_set_paused(RID p_agent, bool p_paused) override;
|
||||
virtual bool agent_get_paused(RID p_agent) const override;
|
||||
|
||||
virtual void agent_set_avoidance_enabled(RID p_agent, bool p_enabled) override;
|
||||
virtual bool agent_get_avoidance_enabled(RID p_agent) const override;
|
||||
|
||||
/// The maximum distance (center point to
|
||||
/// center point) to other agents this agent
|
||||
/// takes into account in the navigation. The
|
||||
/// larger this number, the longer the running
|
||||
/// time of the simulation. If the number is too
|
||||
/// low, the simulation will not be safe.
|
||||
/// Must be non-negative.
|
||||
virtual void agent_set_neighbor_distance(RID p_agent, real_t p_distance) override;
|
||||
virtual real_t agent_get_neighbor_distance(RID p_agent) const override;
|
||||
|
||||
/// The maximum number of other agents this
|
||||
/// agent takes into account in the navigation.
|
||||
/// The larger this number, the longer the
|
||||
/// running time of the simulation. If the
|
||||
/// number is too low, the simulation will not
|
||||
/// be safe.
|
||||
virtual void agent_set_max_neighbors(RID p_agent, int p_count) override;
|
||||
virtual int agent_get_max_neighbors(RID p_agent) const override;
|
||||
|
||||
/// The minimal amount of time for which this
|
||||
/// agent's velocities that are computed by the
|
||||
/// simulation are safe with respect to other
|
||||
/// agents. The larger this number, the sooner
|
||||
/// this agent will respond to the presence of
|
||||
/// other agents, but the less freedom this
|
||||
/// agent has in choosing its velocities.
|
||||
/// Must be positive.
|
||||
virtual void agent_set_time_horizon_agents(RID p_agent, real_t p_time_horizon) override;
|
||||
virtual real_t agent_get_time_horizon_agents(RID p_agent) const override;
|
||||
virtual void agent_set_time_horizon_obstacles(RID p_agent, real_t p_time_horizon) override;
|
||||
virtual real_t agent_get_time_horizon_obstacles(RID p_agent) const override;
|
||||
|
||||
/// The radius of this agent.
|
||||
/// Must be non-negative.
|
||||
virtual void agent_set_radius(RID p_agent, real_t p_radius) override;
|
||||
virtual real_t agent_get_radius(RID p_agent) const override;
|
||||
|
||||
/// The maximum speed of this agent.
|
||||
/// Must be non-negative.
|
||||
virtual void agent_set_max_speed(RID p_agent, real_t p_max_speed) override;
|
||||
virtual real_t agent_get_max_speed(RID p_agent) const override;
|
||||
|
||||
/// forces and agent velocity change in the avoidance simulation, adds simulation instability if done recklessly
|
||||
virtual void agent_set_velocity_forced(RID p_agent, Vector2 p_velocity) override;
|
||||
|
||||
/// The wanted velocity for the agent as a "suggestion" to the avoidance simulation.
|
||||
/// The simulation will try to fulfill this velocity wish if possible but may change the velocity depending on other agent's and obstacles'.
|
||||
virtual void agent_set_velocity(RID p_agent, Vector2 p_velocity) override;
|
||||
virtual Vector2 agent_get_velocity(RID p_agent) const override;
|
||||
|
||||
/// Position of the agent in world space.
|
||||
virtual void agent_set_position(RID p_agent, Vector2 p_position) override;
|
||||
virtual Vector2 agent_get_position(RID p_agent) const override;
|
||||
|
||||
/// Returns true if the map got changed the previous frame.
|
||||
virtual bool agent_is_map_changed(RID p_agent) const override;
|
||||
|
||||
/// Callback called at the end of the RVO process
|
||||
virtual void agent_set_avoidance_callback(RID p_agent, Callable p_callback) override;
|
||||
virtual bool agent_has_avoidance_callback(RID p_agent) const override;
|
||||
|
||||
virtual void agent_set_avoidance_layers(RID p_agent, uint32_t p_layers) override;
|
||||
virtual uint32_t agent_get_avoidance_layers(RID p_agent) const override;
|
||||
|
||||
virtual void agent_set_avoidance_mask(RID p_agent, uint32_t p_mask) override;
|
||||
virtual uint32_t agent_get_avoidance_mask(RID p_agent) const override;
|
||||
|
||||
virtual void agent_set_avoidance_priority(RID p_agent, real_t p_priority) override;
|
||||
virtual real_t agent_get_avoidance_priority(RID p_agent) const override;
|
||||
|
||||
virtual RID obstacle_create() override;
|
||||
virtual void obstacle_set_avoidance_enabled(RID p_obstacle, bool p_enabled) override;
|
||||
virtual bool obstacle_get_avoidance_enabled(RID p_obstacle) const override;
|
||||
virtual void obstacle_set_map(RID p_obstacle, RID p_map) override;
|
||||
virtual RID obstacle_get_map(RID p_obstacle) const override;
|
||||
virtual void obstacle_set_paused(RID p_obstacle, bool p_paused) override;
|
||||
virtual bool obstacle_get_paused(RID p_obstacle) const override;
|
||||
virtual void obstacle_set_radius(RID p_obstacle, real_t p_radius) override;
|
||||
virtual real_t obstacle_get_radius(RID p_obstacle) const override;
|
||||
virtual void obstacle_set_velocity(RID p_obstacle, Vector2 p_velocity) override;
|
||||
virtual Vector2 obstacle_get_velocity(RID p_obstacle) const override;
|
||||
virtual void obstacle_set_position(RID p_obstacle, Vector2 p_position) override;
|
||||
virtual Vector2 obstacle_get_position(RID p_obstacle) const override;
|
||||
virtual void obstacle_set_vertices(RID p_obstacle, const Vector<Vector2> &p_vertices) override;
|
||||
virtual Vector<Vector2> obstacle_get_vertices(RID p_obstacle) const override;
|
||||
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 Callable &p_callback) override;
|
||||
|
||||
virtual void init() override;
|
||||
virtual void sync() override;
|
||||
virtual void finish() override;
|
||||
virtual void free(RID p_object) override;
|
||||
|
||||
virtual void 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 = Callable()) override;
|
||||
virtual void bake_from_source_geometry_data(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
|
||||
virtual void bake_from_source_geometry_data_async(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
|
||||
virtual bool is_baking_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon) const override;
|
||||
|
||||
virtual RID source_geometry_parser_create() override;
|
||||
virtual void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) override;
|
||||
|
||||
virtual Vector<Vector2> simplify_path(const Vector<Vector2> &p_path, real_t p_epsilon) override;
|
||||
};
|
||||
|
||||
#endif // GODOT_NAVIGATION_SERVER_2D_H
|
||||
525
engine/modules/navigation/2d/nav_mesh_generator_2d.cpp
Normal file
525
engine/modules/navigation/2d/nav_mesh_generator_2d.cpp
Normal file
|
|
@ -0,0 +1,525 @@
|
|||
/**************************************************************************/
|
||||
/* nav_mesh_generator_2d.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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#ifdef CLIPPER2_ENABLED
|
||||
|
||||
#include "nav_mesh_generator_2d.h"
|
||||
|
||||
#include "core/config/project_settings.h"
|
||||
#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h"
|
||||
#include "scene/resources/2d/navigation_polygon.h"
|
||||
|
||||
#include "thirdparty/clipper2/include/clipper2/clipper.h"
|
||||
#include "thirdparty/misc/polypartition.h"
|
||||
|
||||
NavMeshGenerator2D *NavMeshGenerator2D::singleton = nullptr;
|
||||
Mutex NavMeshGenerator2D::baking_navmesh_mutex;
|
||||
Mutex NavMeshGenerator2D::generator_task_mutex;
|
||||
RWLock NavMeshGenerator2D::generator_parsers_rwlock;
|
||||
bool NavMeshGenerator2D::use_threads = true;
|
||||
bool NavMeshGenerator2D::baking_use_multiple_threads = true;
|
||||
bool NavMeshGenerator2D::baking_use_high_priority_threads = true;
|
||||
HashSet<Ref<NavigationPolygon>> NavMeshGenerator2D::baking_navmeshes;
|
||||
HashMap<WorkerThreadPool::TaskID, NavMeshGenerator2D::NavMeshGeneratorTask2D *> NavMeshGenerator2D::generator_tasks;
|
||||
LocalVector<NavMeshGeometryParser2D *> NavMeshGenerator2D::generator_parsers;
|
||||
|
||||
NavMeshGenerator2D *NavMeshGenerator2D::get_singleton() {
|
||||
return singleton;
|
||||
}
|
||||
|
||||
NavMeshGenerator2D::NavMeshGenerator2D() {
|
||||
ERR_FAIL_COND(singleton != nullptr);
|
||||
singleton = this;
|
||||
|
||||
baking_use_multiple_threads = GLOBAL_GET("navigation/baking/thread_model/baking_use_multiple_threads");
|
||||
baking_use_high_priority_threads = GLOBAL_GET("navigation/baking/thread_model/baking_use_high_priority_threads");
|
||||
|
||||
// Using threads might cause problems on certain exports or with the Editor on certain devices.
|
||||
// This is the main switch to turn threaded navmesh baking off should the need arise.
|
||||
use_threads = baking_use_multiple_threads;
|
||||
}
|
||||
|
||||
NavMeshGenerator2D::~NavMeshGenerator2D() {
|
||||
cleanup();
|
||||
}
|
||||
|
||||
void NavMeshGenerator2D::sync() {
|
||||
if (generator_tasks.size() == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
|
||||
{
|
||||
MutexLock generator_task_lock(generator_task_mutex);
|
||||
|
||||
LocalVector<WorkerThreadPool::TaskID> finished_task_ids;
|
||||
|
||||
for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask2D *> &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);
|
||||
|
||||
NavMeshGeneratorTask2D *generator_task = E.value;
|
||||
DEV_ASSERT(generator_task->status == NavMeshGeneratorTask2D::TaskStatus::BAKING_FINISHED);
|
||||
|
||||
baking_navmeshes.erase(generator_task->navigation_mesh);
|
||||
if (generator_task->callback.is_valid()) {
|
||||
generator_emit_callback(generator_task->callback);
|
||||
}
|
||||
memdelete(generator_task);
|
||||
}
|
||||
}
|
||||
|
||||
for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) {
|
||||
generator_tasks.erase(finished_task_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator2D::cleanup() {
|
||||
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
|
||||
{
|
||||
MutexLock generator_task_lock(generator_task_mutex);
|
||||
|
||||
baking_navmeshes.clear();
|
||||
|
||||
for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask2D *> &E : generator_tasks) {
|
||||
WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
|
||||
NavMeshGeneratorTask2D *generator_task = E.value;
|
||||
memdelete(generator_task);
|
||||
}
|
||||
generator_tasks.clear();
|
||||
|
||||
generator_parsers_rwlock.write_lock();
|
||||
generator_parsers.clear();
|
||||
generator_parsers_rwlock.write_unlock();
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator2D::finish() {
|
||||
cleanup();
|
||||
}
|
||||
|
||||
void NavMeshGenerator2D::parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> 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_null());
|
||||
ERR_FAIL_NULL(p_root_node);
|
||||
ERR_FAIL_COND(!p_root_node->is_inside_tree());
|
||||
ERR_FAIL_COND(p_source_geometry_data.is_null());
|
||||
|
||||
generator_parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node);
|
||||
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator2D::bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback) {
|
||||
ERR_FAIL_COND(p_navigation_mesh.is_null());
|
||||
ERR_FAIL_COND(p_source_geometry_data.is_null());
|
||||
|
||||
if (p_navigation_mesh->get_outline_count() == 0 && !p_source_geometry_data->has_data()) {
|
||||
p_navigation_mesh->clear();
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (is_baking(p_navigation_mesh)) {
|
||||
ERR_FAIL_MSG("NavigationPolygon is already baking. Wait for current bake to finish.");
|
||||
}
|
||||
baking_navmesh_mutex.lock();
|
||||
baking_navmeshes.insert(p_navigation_mesh);
|
||||
baking_navmesh_mutex.unlock();
|
||||
|
||||
generator_bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data);
|
||||
|
||||
baking_navmesh_mutex.lock();
|
||||
baking_navmeshes.erase(p_navigation_mesh);
|
||||
baking_navmesh_mutex.unlock();
|
||||
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator2D::bake_from_source_geometry_data_async(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback) {
|
||||
ERR_FAIL_COND(p_navigation_mesh.is_null());
|
||||
ERR_FAIL_COND(p_source_geometry_data.is_null());
|
||||
|
||||
if (p_navigation_mesh->get_outline_count() == 0 && !p_source_geometry_data->has_data()) {
|
||||
p_navigation_mesh->clear();
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (!use_threads) {
|
||||
bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
|
||||
return;
|
||||
}
|
||||
|
||||
if (is_baking(p_navigation_mesh)) {
|
||||
ERR_FAIL_MSG("NavigationPolygon is already baking. Wait for current bake to finish.");
|
||||
}
|
||||
baking_navmesh_mutex.lock();
|
||||
baking_navmeshes.insert(p_navigation_mesh);
|
||||
baking_navmesh_mutex.unlock();
|
||||
|
||||
MutexLock generator_task_lock(generator_task_mutex);
|
||||
NavMeshGeneratorTask2D *generator_task = memnew(NavMeshGeneratorTask2D);
|
||||
generator_task->navigation_mesh = p_navigation_mesh;
|
||||
generator_task->source_geometry_data = p_source_geometry_data;
|
||||
generator_task->callback = p_callback;
|
||||
generator_task->status = NavMeshGeneratorTask2D::TaskStatus::BAKING_STARTED;
|
||||
generator_task->thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMeshGenerator2D::generator_thread_bake, generator_task, NavMeshGenerator2D::baking_use_high_priority_threads, "NavMeshGeneratorBake2D");
|
||||
generator_tasks.insert(generator_task->thread_task_id, generator_task);
|
||||
}
|
||||
|
||||
bool NavMeshGenerator2D::is_baking(Ref<NavigationPolygon> p_navigation_polygon) {
|
||||
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
|
||||
return baking_navmeshes.has(p_navigation_polygon);
|
||||
}
|
||||
|
||||
void NavMeshGenerator2D::generator_thread_bake(void *p_arg) {
|
||||
NavMeshGeneratorTask2D *generator_task = static_cast<NavMeshGeneratorTask2D *>(p_arg);
|
||||
|
||||
generator_bake_from_source_geometry_data(generator_task->navigation_mesh, generator_task->source_geometry_data);
|
||||
|
||||
generator_task->status = NavMeshGeneratorTask2D::TaskStatus::BAKING_FINISHED;
|
||||
}
|
||||
|
||||
void NavMeshGenerator2D::generator_parse_geometry_node(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node, bool p_recurse_children) {
|
||||
generator_parsers_rwlock.read_lock();
|
||||
for (const NavMeshGeometryParser2D *parser : generator_parsers) {
|
||||
if (!parser->callback.is_valid()) {
|
||||
continue;
|
||||
}
|
||||
parser->callback.call(p_navigation_mesh, p_source_geometry_data, p_node);
|
||||
}
|
||||
generator_parsers_rwlock.read_unlock();
|
||||
|
||||
if (p_recurse_children) {
|
||||
for (int i = 0; i < p_node->get_child_count(); i++) {
|
||||
generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, p_node->get_child(i), p_recurse_children);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator2D::set_generator_parsers(LocalVector<NavMeshGeometryParser2D *> p_parsers) {
|
||||
RWLockWrite write_lock(generator_parsers_rwlock);
|
||||
generator_parsers = p_parsers;
|
||||
}
|
||||
|
||||
void NavMeshGenerator2D::generator_parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node) {
|
||||
List<Node *> parse_nodes;
|
||||
|
||||
if (p_navigation_mesh->get_source_geometry_mode() == NavigationPolygon::SOURCE_GEOMETRY_ROOT_NODE_CHILDREN) {
|
||||
parse_nodes.push_back(p_root_node);
|
||||
} else {
|
||||
p_root_node->get_tree()->get_nodes_in_group(p_navigation_mesh->get_source_geometry_group_name(), &parse_nodes);
|
||||
}
|
||||
|
||||
Transform2D root_node_transform = Transform2D();
|
||||
if (Object::cast_to<Node2D>(p_root_node)) {
|
||||
root_node_transform = Object::cast_to<Node2D>(p_root_node)->get_global_transform().affine_inverse();
|
||||
}
|
||||
|
||||
p_source_geometry_data->clear();
|
||||
p_source_geometry_data->root_node_transform = root_node_transform;
|
||||
|
||||
bool recurse_children = p_navigation_mesh->get_source_geometry_mode() != NavigationPolygon::SOURCE_GEOMETRY_GROUPS_EXPLICIT;
|
||||
|
||||
for (Node *E : parse_nodes) {
|
||||
generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, E, recurse_children);
|
||||
}
|
||||
}
|
||||
|
||||
static void generator_recursive_process_polytree_items(List<TPPLPoly> &p_tppl_in_polygon, const Clipper2Lib::PolyPathD *p_polypath_item) {
|
||||
using namespace Clipper2Lib;
|
||||
|
||||
TPPLPoly tp;
|
||||
int size = p_polypath_item->Polygon().size();
|
||||
tp.Init(size);
|
||||
|
||||
int j = 0;
|
||||
for (const PointD &polypath_point : p_polypath_item->Polygon()) {
|
||||
tp[j] = Vector2(static_cast<real_t>(polypath_point.x), static_cast<real_t>(polypath_point.y));
|
||||
++j;
|
||||
}
|
||||
|
||||
if (p_polypath_item->IsHole()) {
|
||||
tp.SetOrientation(TPPL_ORIENTATION_CW);
|
||||
tp.SetHole(true);
|
||||
} else {
|
||||
tp.SetOrientation(TPPL_ORIENTATION_CCW);
|
||||
}
|
||||
p_tppl_in_polygon.push_back(tp);
|
||||
|
||||
for (size_t i = 0; i < p_polypath_item->Count(); i++) {
|
||||
const PolyPathD *polypath_item = p_polypath_item->Child(i);
|
||||
generator_recursive_process_polytree_items(p_tppl_in_polygon, polypath_item);
|
||||
}
|
||||
}
|
||||
|
||||
bool NavMeshGenerator2D::generator_emit_callback(const Callable &p_callback) {
|
||||
ERR_FAIL_COND_V(!p_callback.is_valid(), false);
|
||||
|
||||
Callable::CallError ce;
|
||||
Variant result;
|
||||
p_callback.callp(nullptr, 0, result, ce);
|
||||
|
||||
return ce.error == Callable::CallError::CALL_OK;
|
||||
}
|
||||
|
||||
void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data) {
|
||||
if (p_navigation_mesh.is_null() || p_source_geometry_data.is_null()) {
|
||||
return;
|
||||
}
|
||||
|
||||
using namespace Clipper2Lib;
|
||||
PathsD traversable_polygon_paths;
|
||||
PathsD obstruction_polygon_paths;
|
||||
bool empty_projected_obstructions = true;
|
||||
{
|
||||
RWLockRead read_lock(p_source_geometry_data->geometry_rwlock);
|
||||
|
||||
const Vector<Vector<Vector2>> &traversable_outlines = p_source_geometry_data->traversable_outlines;
|
||||
int outline_count = p_navigation_mesh->get_outline_count();
|
||||
|
||||
if (outline_count == 0 && (!p_source_geometry_data->has_data() || (traversable_outlines.is_empty()))) {
|
||||
return;
|
||||
}
|
||||
|
||||
const Vector<Vector<Vector2>> &obstruction_outlines = p_source_geometry_data->obstruction_outlines;
|
||||
const Vector<NavigationMeshSourceGeometryData2D::ProjectedObstruction> &projected_obstructions = p_source_geometry_data->_projected_obstructions;
|
||||
|
||||
traversable_polygon_paths.reserve(outline_count + traversable_outlines.size());
|
||||
obstruction_polygon_paths.reserve(obstruction_outlines.size());
|
||||
|
||||
for (int i = 0; i < outline_count; i++) {
|
||||
const Vector<Vector2> &traversable_outline = p_navigation_mesh->get_outline(i);
|
||||
PathD subject_path;
|
||||
subject_path.reserve(traversable_outline.size());
|
||||
for (const Vector2 &traversable_point : traversable_outline) {
|
||||
subject_path.emplace_back(traversable_point.x, traversable_point.y);
|
||||
}
|
||||
traversable_polygon_paths.push_back(std::move(subject_path));
|
||||
}
|
||||
|
||||
for (const Vector<Vector2> &traversable_outline : traversable_outlines) {
|
||||
PathD subject_path;
|
||||
subject_path.reserve(traversable_outline.size());
|
||||
for (const Vector2 &traversable_point : traversable_outline) {
|
||||
subject_path.emplace_back(traversable_point.x, traversable_point.y);
|
||||
}
|
||||
traversable_polygon_paths.push_back(std::move(subject_path));
|
||||
}
|
||||
|
||||
empty_projected_obstructions = projected_obstructions.is_empty();
|
||||
if (!empty_projected_obstructions) {
|
||||
for (const NavigationMeshSourceGeometryData2D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
|
||||
if (projected_obstruction.carve) {
|
||||
continue;
|
||||
}
|
||||
if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 2 != 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
PathD clip_path;
|
||||
clip_path.reserve(projected_obstruction.vertices.size() / 2);
|
||||
for (int i = 0; i < projected_obstruction.vertices.size() / 2; i++) {
|
||||
clip_path.emplace_back(projected_obstruction.vertices[i * 2], projected_obstruction.vertices[i * 2 + 1]);
|
||||
}
|
||||
if (!IsPositive(clip_path)) {
|
||||
std::reverse(clip_path.begin(), clip_path.end());
|
||||
}
|
||||
obstruction_polygon_paths.push_back(std::move(clip_path));
|
||||
}
|
||||
}
|
||||
|
||||
for (const Vector<Vector2> &obstruction_outline : obstruction_outlines) {
|
||||
PathD clip_path;
|
||||
clip_path.reserve(obstruction_outline.size());
|
||||
for (const Vector2 &obstruction_point : obstruction_outline) {
|
||||
clip_path.emplace_back(obstruction_point.x, obstruction_point.y);
|
||||
}
|
||||
obstruction_polygon_paths.push_back(std::move(clip_path));
|
||||
}
|
||||
}
|
||||
|
||||
Rect2 baking_rect = p_navigation_mesh->get_baking_rect();
|
||||
if (baking_rect.has_area()) {
|
||||
Vector2 baking_rect_offset = p_navigation_mesh->get_baking_rect_offset();
|
||||
|
||||
const int rect_begin_x = baking_rect.position[0] + baking_rect_offset.x;
|
||||
const int rect_begin_y = baking_rect.position[1] + baking_rect_offset.y;
|
||||
const int rect_end_x = baking_rect.position[0] + baking_rect.size[0] + baking_rect_offset.x;
|
||||
const int rect_end_y = baking_rect.position[1] + baking_rect.size[1] + baking_rect_offset.y;
|
||||
|
||||
RectD clipper_rect = RectD(rect_begin_x, rect_begin_y, rect_end_x, rect_end_y);
|
||||
|
||||
traversable_polygon_paths = RectClip(clipper_rect, traversable_polygon_paths);
|
||||
obstruction_polygon_paths = RectClip(clipper_rect, obstruction_polygon_paths);
|
||||
}
|
||||
|
||||
// first merge all traversable polygons according to user specified fill rule
|
||||
PathsD dummy_clip_path;
|
||||
traversable_polygon_paths = Union(traversable_polygon_paths, dummy_clip_path, FillRule::NonZero);
|
||||
// merge all obstruction polygons, don't allow holes for what is considered "solid" 2D geometry
|
||||
obstruction_polygon_paths = Union(obstruction_polygon_paths, dummy_clip_path, FillRule::NonZero);
|
||||
|
||||
PathsD path_solution = Difference(traversable_polygon_paths, obstruction_polygon_paths, FillRule::NonZero);
|
||||
|
||||
real_t agent_radius_offset = p_navigation_mesh->get_agent_radius();
|
||||
if (agent_radius_offset > 0.0) {
|
||||
path_solution = InflatePaths(path_solution, -agent_radius_offset, JoinType::Miter, EndType::Polygon);
|
||||
}
|
||||
|
||||
// Apply obstructions that are not affected by agent radius, the ones with carve enabled.
|
||||
if (!empty_projected_obstructions) {
|
||||
RWLockRead read_lock(p_source_geometry_data->geometry_rwlock);
|
||||
const Vector<NavigationMeshSourceGeometryData2D::ProjectedObstruction> &projected_obstructions = p_source_geometry_data->_projected_obstructions;
|
||||
obstruction_polygon_paths.resize(0);
|
||||
for (const NavigationMeshSourceGeometryData2D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
|
||||
if (!projected_obstruction.carve) {
|
||||
continue;
|
||||
}
|
||||
if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 2 != 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
PathD clip_path;
|
||||
clip_path.reserve(projected_obstruction.vertices.size() / 2);
|
||||
for (int i = 0; i < projected_obstruction.vertices.size() / 2; i++) {
|
||||
clip_path.emplace_back(projected_obstruction.vertices[i * 2], projected_obstruction.vertices[i * 2 + 1]);
|
||||
}
|
||||
if (!IsPositive(clip_path)) {
|
||||
std::reverse(clip_path.begin(), clip_path.end());
|
||||
}
|
||||
obstruction_polygon_paths.push_back(std::move(clip_path));
|
||||
}
|
||||
if (obstruction_polygon_paths.size() > 0) {
|
||||
path_solution = Difference(path_solution, obstruction_polygon_paths, FillRule::NonZero);
|
||||
}
|
||||
}
|
||||
|
||||
//path_solution = RamerDouglasPeucker(path_solution, 0.025); //
|
||||
|
||||
real_t border_size = p_navigation_mesh->get_border_size();
|
||||
if (baking_rect.has_area() && border_size > 0.0) {
|
||||
Vector2 baking_rect_offset = p_navigation_mesh->get_baking_rect_offset();
|
||||
|
||||
const int rect_begin_x = baking_rect.position[0] + baking_rect_offset.x + border_size;
|
||||
const int rect_begin_y = baking_rect.position[1] + baking_rect_offset.y + border_size;
|
||||
const int rect_end_x = baking_rect.position[0] + baking_rect.size[0] + baking_rect_offset.x - border_size;
|
||||
const int rect_end_y = baking_rect.position[1] + baking_rect.size[1] + baking_rect_offset.y - border_size;
|
||||
|
||||
RectD clipper_rect = RectD(rect_begin_x, rect_begin_y, rect_end_x, rect_end_y);
|
||||
|
||||
path_solution = RectClip(clipper_rect, path_solution);
|
||||
}
|
||||
|
||||
if (path_solution.size() == 0) {
|
||||
p_navigation_mesh->clear();
|
||||
return;
|
||||
}
|
||||
|
||||
ClipType clipper_cliptype = ClipType::Union;
|
||||
|
||||
List<TPPLPoly> tppl_in_polygon, tppl_out_polygon;
|
||||
|
||||
PolyTreeD polytree;
|
||||
ClipperD clipper_D;
|
||||
|
||||
clipper_D.AddSubject(path_solution);
|
||||
clipper_D.Execute(clipper_cliptype, FillRule::NonZero, polytree);
|
||||
|
||||
for (size_t i = 0; i < polytree.Count(); i++) {
|
||||
const PolyPathD *polypath_item = polytree[i];
|
||||
generator_recursive_process_polytree_items(tppl_in_polygon, polypath_item);
|
||||
}
|
||||
|
||||
TPPLPartition tpart;
|
||||
|
||||
NavigationPolygon::SamplePartitionType sample_partition_type = p_navigation_mesh->get_sample_partition_type();
|
||||
|
||||
switch (sample_partition_type) {
|
||||
case NavigationPolygon::SamplePartitionType::SAMPLE_PARTITION_CONVEX_PARTITION:
|
||||
if (tpart.ConvexPartition_HM(&tppl_in_polygon, &tppl_out_polygon) == 0) {
|
||||
ERR_PRINT("NavigationPolygon polygon convex partition failed. Unable to create a valid navigation mesh polygon layout from provided source geometry.");
|
||||
p_navigation_mesh->set_vertices(Vector<Vector2>());
|
||||
p_navigation_mesh->clear_polygons();
|
||||
return;
|
||||
}
|
||||
break;
|
||||
case NavigationPolygon::SamplePartitionType::SAMPLE_PARTITION_TRIANGULATE:
|
||||
if (tpart.Triangulate_EC(&tppl_in_polygon, &tppl_out_polygon) == 0) {
|
||||
ERR_PRINT("NavigationPolygon polygon triangulation failed. Unable to create a valid navigation mesh polygon layout from provided source geometry.");
|
||||
p_navigation_mesh->set_vertices(Vector<Vector2>());
|
||||
p_navigation_mesh->clear_polygons();
|
||||
return;
|
||||
}
|
||||
break;
|
||||
default: {
|
||||
ERR_PRINT("NavigationPolygon polygon partitioning failed. Unrecognized partition type.");
|
||||
p_navigation_mesh->set_vertices(Vector<Vector2>());
|
||||
p_navigation_mesh->clear_polygons();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
Vector<Vector2> new_vertices;
|
||||
Vector<Vector<int>> new_polygons;
|
||||
|
||||
HashMap<Vector2, int> points;
|
||||
for (List<TPPLPoly>::Element *I = tppl_out_polygon.front(); I; I = I->next()) {
|
||||
TPPLPoly &tp = I->get();
|
||||
|
||||
Vector<int> new_polygon;
|
||||
|
||||
for (int64_t i = 0; i < tp.GetNumPoints(); i++) {
|
||||
HashMap<Vector2, int>::Iterator E = points.find(tp[i]);
|
||||
if (!E) {
|
||||
E = points.insert(tp[i], new_vertices.size());
|
||||
new_vertices.push_back(tp[i]);
|
||||
}
|
||||
new_polygon.push_back(E->value);
|
||||
}
|
||||
|
||||
new_polygons.push_back(new_polygon);
|
||||
}
|
||||
|
||||
p_navigation_mesh->set_data(new_vertices, new_polygons);
|
||||
}
|
||||
|
||||
#endif // CLIPPER2_ENABLED
|
||||
106
engine/modules/navigation/2d/nav_mesh_generator_2d.h
Normal file
106
engine/modules/navigation/2d/nav_mesh_generator_2d.h
Normal file
|
|
@ -0,0 +1,106 @@
|
|||
/**************************************************************************/
|
||||
/* nav_mesh_generator_2d.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_GENERATOR_2D_H
|
||||
#define NAV_MESH_GENERATOR_2D_H
|
||||
|
||||
#ifdef CLIPPER2_ENABLED
|
||||
|
||||
#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;
|
||||
class NavigationMeshSourceGeometryData2D;
|
||||
|
||||
class NavMeshGenerator2D : public Object {
|
||||
static NavMeshGenerator2D *singleton;
|
||||
|
||||
static Mutex baking_navmesh_mutex;
|
||||
static Mutex generator_task_mutex;
|
||||
|
||||
static RWLock generator_parsers_rwlock;
|
||||
static LocalVector<NavMeshGeometryParser2D *> generator_parsers;
|
||||
|
||||
static bool use_threads;
|
||||
static bool baking_use_multiple_threads;
|
||||
static bool baking_use_high_priority_threads;
|
||||
|
||||
struct NavMeshGeneratorTask2D {
|
||||
enum TaskStatus {
|
||||
BAKING_STARTED,
|
||||
BAKING_FINISHED,
|
||||
BAKING_FAILED,
|
||||
CALLBACK_DISPATCHED,
|
||||
CALLBACK_FAILED,
|
||||
};
|
||||
|
||||
Ref<NavigationPolygon> navigation_mesh;
|
||||
Ref<NavigationMeshSourceGeometryData2D> source_geometry_data;
|
||||
Callable callback;
|
||||
WorkerThreadPool::TaskID thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
|
||||
NavMeshGeneratorTask2D::TaskStatus status = NavMeshGeneratorTask2D::TaskStatus::BAKING_STARTED;
|
||||
};
|
||||
|
||||
static HashMap<WorkerThreadPool::TaskID, NavMeshGeneratorTask2D *> generator_tasks;
|
||||
|
||||
static void generator_thread_bake(void *p_arg);
|
||||
|
||||
static HashSet<Ref<NavigationPolygon>> baking_navmeshes;
|
||||
|
||||
static void generator_parse_geometry_node(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node, bool p_recurse_children);
|
||||
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 bool generator_emit_callback(const Callable &p_callback);
|
||||
|
||||
public:
|
||||
static NavMeshGenerator2D *get_singleton();
|
||||
|
||||
static void sync();
|
||||
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);
|
||||
|
||||
NavMeshGenerator2D();
|
||||
~NavMeshGenerator2D();
|
||||
};
|
||||
|
||||
#endif // CLIPPER2_ENABLED
|
||||
|
||||
#endif // NAV_MESH_GENERATOR_2D_H
|
||||
1539
engine/modules/navigation/3d/godot_navigation_server_3d.cpp
Normal file
1539
engine/modules/navigation/3d/godot_navigation_server_3d.cpp
Normal file
File diff suppressed because it is too large
Load diff
305
engine/modules/navigation/3d/godot_navigation_server_3d.h
Normal file
305
engine/modules/navigation/3d/godot_navigation_server_3d.h
Normal file
|
|
@ -0,0 +1,305 @@
|
|||
/**************************************************************************/
|
||||
/* godot_navigation_server_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 GODOT_NAVIGATION_SERVER_3D_H
|
||||
#define GODOT_NAVIGATION_SERVER_3D_H
|
||||
|
||||
#include "../nav_agent.h"
|
||||
#include "../nav_link.h"
|
||||
#include "../nav_map.h"
|
||||
#include "../nav_obstacle.h"
|
||||
#include "../nav_region.h"
|
||||
|
||||
#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.
|
||||
|
||||
#define MERGE_INTERNAL(A, B) A##B
|
||||
#define MERGE(A, B) MERGE_INTERNAL(A, B)
|
||||
|
||||
#define COMMAND_1(F_NAME, T_0, D_0) \
|
||||
virtual void F_NAME(T_0 D_0) override; \
|
||||
void MERGE(_cmd_, F_NAME)(T_0 D_0)
|
||||
|
||||
#define COMMAND_2(F_NAME, T_0, D_0, T_1, D_1) \
|
||||
virtual void F_NAME(T_0 D_0, T_1 D_1) override; \
|
||||
void MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1)
|
||||
|
||||
class GodotNavigationServer3D;
|
||||
#ifndef _3D_DISABLED
|
||||
class NavMeshGenerator3D;
|
||||
#endif // _3D_DISABLED
|
||||
|
||||
struct SetCommand {
|
||||
virtual ~SetCommand() {}
|
||||
virtual void exec(GodotNavigationServer3D *server) = 0;
|
||||
};
|
||||
|
||||
class GodotNavigationServer3D : public NavigationServer3D {
|
||||
Mutex commands_mutex;
|
||||
/// Mutex used to make any operation threadsafe.
|
||||
Mutex operations_mutex;
|
||||
|
||||
LocalVector<SetCommand *> commands;
|
||||
|
||||
mutable RID_Owner<NavLink> link_owner;
|
||||
mutable RID_Owner<NavMap> map_owner;
|
||||
mutable RID_Owner<NavRegion> region_owner;
|
||||
mutable RID_Owner<NavAgent> agent_owner;
|
||||
mutable RID_Owner<NavObstacle> obstacle_owner;
|
||||
|
||||
bool active = true;
|
||||
LocalVector<NavMap *> active_maps;
|
||||
LocalVector<uint32_t> active_maps_iteration_id;
|
||||
|
||||
#ifndef _3D_DISABLED
|
||||
NavMeshGenerator3D *navmesh_generator_3d = nullptr;
|
||||
#endif // _3D_DISABLED
|
||||
|
||||
// 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;
|
||||
int pm_obstacle_count = 0;
|
||||
|
||||
public:
|
||||
GodotNavigationServer3D();
|
||||
virtual ~GodotNavigationServer3D();
|
||||
|
||||
void add_command(SetCommand *command);
|
||||
|
||||
virtual TypedArray<RID> get_maps() const override;
|
||||
|
||||
virtual RID map_create() override;
|
||||
COMMAND_2(map_set_active, RID, p_map, bool, p_active);
|
||||
virtual bool map_is_active(RID p_map) const override;
|
||||
|
||||
COMMAND_2(map_set_up, RID, p_map, Vector3, p_up);
|
||||
virtual Vector3 map_get_up(RID p_map) const override;
|
||||
|
||||
COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size);
|
||||
virtual real_t map_get_cell_size(RID p_map) const override;
|
||||
|
||||
COMMAND_2(map_set_cell_height, RID, p_map, real_t, p_cell_height);
|
||||
virtual real_t map_get_cell_height(RID p_map) const override;
|
||||
|
||||
COMMAND_2(map_set_merge_rasterizer_cell_scale, RID, p_map, float, p_value);
|
||||
virtual float map_get_merge_rasterizer_cell_scale(RID p_map) const override;
|
||||
|
||||
COMMAND_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled);
|
||||
virtual bool map_get_use_edge_connections(RID p_map) const override;
|
||||
|
||||
COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin);
|
||||
virtual real_t map_get_edge_connection_margin(RID p_map) const override;
|
||||
|
||||
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) 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;
|
||||
virtual Vector3 map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const override;
|
||||
virtual RID map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const override;
|
||||
|
||||
virtual TypedArray<RID> map_get_links(RID p_map) const override;
|
||||
virtual TypedArray<RID> map_get_regions(RID p_map) const override;
|
||||
virtual TypedArray<RID> map_get_agents(RID p_map) const override;
|
||||
virtual TypedArray<RID> map_get_obstacles(RID p_map) const override;
|
||||
|
||||
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;
|
||||
|
||||
COMMAND_2(region_set_enabled, RID, p_region, bool, p_enabled);
|
||||
virtual bool region_get_enabled(RID p_region) const override;
|
||||
|
||||
COMMAND_2(region_set_use_edge_connections, RID, p_region, bool, p_enabled);
|
||||
virtual bool region_get_use_edge_connections(RID p_region) const override;
|
||||
|
||||
COMMAND_2(region_set_enter_cost, RID, p_region, real_t, p_enter_cost);
|
||||
virtual real_t region_get_enter_cost(RID p_region) const override;
|
||||
COMMAND_2(region_set_travel_cost, RID, p_region, real_t, p_travel_cost);
|
||||
virtual real_t region_get_travel_cost(RID p_region) const override;
|
||||
|
||||
COMMAND_2(region_set_owner_id, RID, p_region, ObjectID, p_owner_id);
|
||||
virtual ObjectID region_get_owner_id(RID p_region) const override;
|
||||
|
||||
virtual bool region_owns_point(RID p_region, const Vector3 &p_point) const override;
|
||||
|
||||
COMMAND_2(region_set_map, RID, p_region, RID, p_map);
|
||||
virtual RID region_get_map(RID p_region) const override;
|
||||
COMMAND_2(region_set_navigation_layers, RID, p_region, uint32_t, p_navigation_layers);
|
||||
virtual uint32_t region_get_navigation_layers(RID p_region) const override;
|
||||
COMMAND_2(region_set_transform, RID, p_region, Transform3D, p_transform);
|
||||
virtual Transform3D region_get_transform(RID p_region) const override;
|
||||
COMMAND_2(region_set_navigation_mesh, RID, p_region, Ref<NavigationMesh>, p_navigation_mesh);
|
||||
#ifndef DISABLE_DEPRECATED
|
||||
virtual void region_bake_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh, Node *p_root_node) override;
|
||||
#endif // DISABLE_DEPRECATED
|
||||
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);
|
||||
virtual RID link_get_map(RID p_link) const override;
|
||||
COMMAND_2(link_set_enabled, RID, p_link, bool, p_enabled);
|
||||
virtual bool link_get_enabled(RID p_link) const override;
|
||||
COMMAND_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional);
|
||||
virtual bool link_is_bidirectional(RID p_link) const override;
|
||||
COMMAND_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers);
|
||||
virtual uint32_t link_get_navigation_layers(RID p_link) const override;
|
||||
COMMAND_2(link_set_start_position, RID, p_link, Vector3, p_position);
|
||||
virtual Vector3 link_get_start_position(RID p_link) const override;
|
||||
COMMAND_2(link_set_end_position, RID, p_link, Vector3, p_position);
|
||||
virtual Vector3 link_get_end_position(RID p_link) const override;
|
||||
COMMAND_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost);
|
||||
virtual real_t link_get_enter_cost(RID p_link) const override;
|
||||
COMMAND_2(link_set_travel_cost, RID, p_link, real_t, p_travel_cost);
|
||||
virtual real_t link_get_travel_cost(RID p_link) const override;
|
||||
COMMAND_2(link_set_owner_id, RID, p_link, ObjectID, p_owner_id);
|
||||
virtual ObjectID link_get_owner_id(RID p_link) const override;
|
||||
|
||||
virtual RID agent_create() override;
|
||||
COMMAND_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled);
|
||||
virtual bool agent_get_avoidance_enabled(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_use_3d_avoidance, RID, p_agent, bool, p_enabled);
|
||||
virtual bool agent_get_use_3d_avoidance(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_map, RID, p_agent, RID, p_map);
|
||||
virtual RID agent_get_map(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_paused, RID, p_agent, bool, p_paused);
|
||||
virtual bool agent_get_paused(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_distance);
|
||||
virtual real_t agent_get_neighbor_distance(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count);
|
||||
virtual int agent_get_max_neighbors(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon);
|
||||
virtual real_t agent_get_time_horizon_agents(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon);
|
||||
virtual real_t agent_get_time_horizon_obstacles(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius);
|
||||
virtual real_t agent_get_radius(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_height, RID, p_agent, real_t, p_height);
|
||||
virtual real_t agent_get_height(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed);
|
||||
virtual real_t agent_get_max_speed(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity);
|
||||
virtual Vector3 agent_get_velocity(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_velocity_forced, RID, p_agent, Vector3, p_velocity);
|
||||
COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position);
|
||||
virtual Vector3 agent_get_position(RID p_agent) const override;
|
||||
virtual bool agent_is_map_changed(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback);
|
||||
virtual bool agent_has_avoidance_callback(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers);
|
||||
virtual uint32_t agent_get_avoidance_layers(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask);
|
||||
virtual uint32_t agent_get_avoidance_mask(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority);
|
||||
virtual real_t agent_get_avoidance_priority(RID p_agent) const override;
|
||||
|
||||
virtual RID obstacle_create() override;
|
||||
COMMAND_2(obstacle_set_avoidance_enabled, RID, p_obstacle, bool, p_enabled);
|
||||
virtual bool obstacle_get_avoidance_enabled(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_use_3d_avoidance, RID, p_obstacle, bool, p_enabled);
|
||||
virtual bool obstacle_get_use_3d_avoidance(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_map, RID, p_obstacle, RID, p_map);
|
||||
virtual RID obstacle_get_map(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused);
|
||||
virtual bool obstacle_get_paused(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius);
|
||||
virtual real_t obstacle_get_radius(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_velocity, RID, p_obstacle, Vector3, p_velocity);
|
||||
virtual Vector3 obstacle_get_velocity(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector3, p_position);
|
||||
virtual Vector3 obstacle_get_position(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_height, RID, p_obstacle, real_t, p_height);
|
||||
virtual real_t obstacle_get_height(RID p_obstacle) const override;
|
||||
virtual void obstacle_set_vertices(RID p_obstacle, const Vector<Vector3> &p_vertices) override;
|
||||
virtual Vector<Vector3> obstacle_get_vertices(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers);
|
||||
virtual uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const override;
|
||||
|
||||
virtual void 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 = Callable()) override;
|
||||
virtual void bake_from_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
|
||||
virtual void bake_from_source_geometry_data_async(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
|
||||
virtual bool is_baking_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) const override;
|
||||
|
||||
virtual RID source_geometry_parser_create() override;
|
||||
virtual void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) override;
|
||||
|
||||
virtual Vector<Vector3> simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) override;
|
||||
|
||||
public:
|
||||
COMMAND_1(free, RID, p_object);
|
||||
|
||||
virtual void set_active(bool p_active) override;
|
||||
|
||||
void flush_queries();
|
||||
virtual void process(real_t p_delta_time) override;
|
||||
virtual void init() override;
|
||||
virtual void sync() override;
|
||||
virtual void finish() 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;
|
||||
|
||||
private:
|
||||
void internal_free_agent(RID p_object);
|
||||
void internal_free_obstacle(RID p_object);
|
||||
};
|
||||
|
||||
#undef COMMAND_1
|
||||
#undef COMMAND_2
|
||||
|
||||
#endif // GODOT_NAVIGATION_SERVER_3D_H
|
||||
57
engine/modules/navigation/3d/nav_base_iteration_3d.h
Normal file
57
engine/modules/navigation/3d/nav_base_iteration_3d.h
Normal 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
409
engine/modules/navigation/3d/nav_map_builder_3d.cpp
Normal 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> ®ions = map_iteration->region_iterations;
|
||||
HashMap<uint32_t, LocalVector<gd::Edge::Connection>> ®ion_external_connections = map_iteration->external_region_connections;
|
||||
|
||||
// Remove regions connections.
|
||||
region_external_connections.clear();
|
||||
for (const NavRegionIteration ®ion : regions) {
|
||||
region_external_connections[region.id] = LocalVector<gd::Edge::Connection>();
|
||||
}
|
||||
|
||||
// Copy all region polygons in the map.
|
||||
int polygon_count = 0;
|
||||
for (NavRegionIteration ®ion : 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 ®ion : 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>> ®ion_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 ®ion : 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
52
engine/modules/navigation/3d/nav_map_builder_3d.h
Normal 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
114
engine/modules/navigation/3d/nav_map_iteration_3d.h
Normal 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
|
||||
564
engine/modules/navigation/3d/nav_mesh_generator_3d.cpp
Normal file
564
engine/modules/navigation/3d/nav_mesh_generator_3d.cpp
Normal file
|
|
@ -0,0 +1,564 @@
|
|||
/**************************************************************************/
|
||||
/* nav_mesh_generator_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_mesh_generator_3d.h"
|
||||
|
||||
#include "core/config/project_settings.h"
|
||||
#include "core/os/thread.h"
|
||||
#include "scene/3d/node_3d.h"
|
||||
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
|
||||
#include "scene/resources/navigation_mesh.h"
|
||||
|
||||
#include <Recast.h>
|
||||
|
||||
NavMeshGenerator3D *NavMeshGenerator3D::singleton = nullptr;
|
||||
Mutex NavMeshGenerator3D::baking_navmesh_mutex;
|
||||
Mutex NavMeshGenerator3D::generator_task_mutex;
|
||||
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;
|
||||
LocalVector<NavMeshGeometryParser3D *> NavMeshGenerator3D::generator_parsers;
|
||||
|
||||
NavMeshGenerator3D *NavMeshGenerator3D::get_singleton() {
|
||||
return singleton;
|
||||
}
|
||||
|
||||
NavMeshGenerator3D::NavMeshGenerator3D() {
|
||||
ERR_FAIL_COND(singleton != nullptr);
|
||||
singleton = this;
|
||||
|
||||
baking_use_multiple_threads = GLOBAL_GET("navigation/baking/thread_model/baking_use_multiple_threads");
|
||||
baking_use_high_priority_threads = GLOBAL_GET("navigation/baking/thread_model/baking_use_high_priority_threads");
|
||||
|
||||
// Using threads might cause problems on certain exports or with the Editor on certain devices.
|
||||
// This is the main switch to turn threaded navmesh baking off should the need arise.
|
||||
use_threads = baking_use_multiple_threads;
|
||||
}
|
||||
|
||||
NavMeshGenerator3D::~NavMeshGenerator3D() {
|
||||
cleanup();
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::sync() {
|
||||
if (generator_tasks.size() == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
|
||||
{
|
||||
MutexLock generator_task_lock(generator_task_mutex);
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
memdelete(generator_task);
|
||||
}
|
||||
}
|
||||
|
||||
for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) {
|
||||
generator_tasks.erase(finished_task_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::cleanup() {
|
||||
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
|
||||
{
|
||||
MutexLock generator_task_lock(generator_task_mutex);
|
||||
|
||||
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);
|
||||
}
|
||||
generator_tasks.clear();
|
||||
|
||||
generator_parsers_rwlock.write_lock();
|
||||
generator_parsers.clear();
|
||||
generator_parsers_rwlock.write_unlock();
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::finish() {
|
||||
cleanup();
|
||||
}
|
||||
|
||||
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_null());
|
||||
ERR_FAIL_NULL(p_root_node);
|
||||
ERR_FAIL_COND(!p_root_node->is_inside_tree());
|
||||
ERR_FAIL_COND(p_source_geometry_data.is_null());
|
||||
|
||||
generator_parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node);
|
||||
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
}
|
||||
|
||||
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_null());
|
||||
ERR_FAIL_COND(p_source_geometry_data.is_null());
|
||||
|
||||
if (!p_source_geometry_data->has_data()) {
|
||||
p_navigation_mesh->clear();
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (is_baking(p_navigation_mesh)) {
|
||||
ERR_FAIL_MSG("NavigationMesh is already baking. Wait for current bake to finish.");
|
||||
}
|
||||
baking_navmesh_mutex.lock();
|
||||
baking_navmeshes.insert(p_navigation_mesh);
|
||||
baking_navmesh_mutex.unlock();
|
||||
|
||||
generator_bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data);
|
||||
|
||||
baking_navmesh_mutex.lock();
|
||||
baking_navmeshes.erase(p_navigation_mesh);
|
||||
baking_navmesh_mutex.unlock();
|
||||
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
}
|
||||
|
||||
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_null());
|
||||
ERR_FAIL_COND(p_source_geometry_data.is_null());
|
||||
|
||||
if (!p_source_geometry_data->has_data()) {
|
||||
p_navigation_mesh->clear();
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (!use_threads) {
|
||||
bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
|
||||
return;
|
||||
}
|
||||
|
||||
if (is_baking(p_navigation_mesh)) {
|
||||
ERR_FAIL_MSG("NavigationMesh is already baking. Wait for current bake to finish.");
|
||||
return;
|
||||
}
|
||||
baking_navmesh_mutex.lock();
|
||||
baking_navmeshes.insert(p_navigation_mesh);
|
||||
baking_navmesh_mutex.unlock();
|
||||
|
||||
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;
|
||||
generator_task->callback = p_callback;
|
||||
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);
|
||||
}
|
||||
|
||||
bool NavMeshGenerator3D::is_baking(Ref<NavigationMesh> p_navigation_mesh) {
|
||||
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
|
||||
return baking_navmeshes.has(p_navigation_mesh);
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::generator_thread_bake(void *p_arg) {
|
||||
NavMeshGeneratorTask3D *generator_task = static_cast<NavMeshGeneratorTask3D *>(p_arg);
|
||||
|
||||
generator_bake_from_source_geometry_data(generator_task->navigation_mesh, generator_task->source_geometry_data);
|
||||
|
||||
generator_task->status = NavMeshGeneratorTask3D::TaskStatus::BAKING_FINISHED;
|
||||
}
|
||||
|
||||
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_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_parsers_rwlock.read_unlock();
|
||||
|
||||
if (p_recurse_children) {
|
||||
for (int i = 0; i < p_node->get_child_count(); i++) {
|
||||
generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, p_node->get_child(i), p_recurse_children);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
List<Node *> parse_nodes;
|
||||
|
||||
if (p_navigation_mesh->get_source_geometry_mode() == NavigationMesh::SOURCE_GEOMETRY_ROOT_NODE_CHILDREN) {
|
||||
parse_nodes.push_back(p_root_node);
|
||||
} else {
|
||||
p_root_node->get_tree()->get_nodes_in_group(p_navigation_mesh->get_source_group_name(), &parse_nodes);
|
||||
}
|
||||
|
||||
Transform3D root_node_transform = Transform3D();
|
||||
if (Object::cast_to<Node3D>(p_root_node)) {
|
||||
root_node_transform = Object::cast_to<Node3D>(p_root_node)->get_global_transform().affine_inverse();
|
||||
}
|
||||
|
||||
p_source_geometry_data->clear();
|
||||
p_source_geometry_data->root_node_transform = root_node_transform;
|
||||
|
||||
bool recurse_children = p_navigation_mesh->get_source_geometry_mode() != NavigationMesh::SOURCE_GEOMETRY_GROUPS_EXPLICIT;
|
||||
|
||||
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()) {
|
||||
return;
|
||||
}
|
||||
|
||||
Vector<float> source_geometry_vertices;
|
||||
Vector<int> source_geometry_indices;
|
||||
Vector<NavigationMeshSourceGeometryData3D::ProjectedObstruction> projected_obstructions;
|
||||
|
||||
p_source_geometry_data->get_data(
|
||||
source_geometry_vertices,
|
||||
source_geometry_indices,
|
||||
projected_obstructions);
|
||||
|
||||
if (source_geometry_vertices.size() < 3 || source_geometry_indices.size() < 3) {
|
||||
return;
|
||||
}
|
||||
|
||||
rcHeightfield *hf = nullptr;
|
||||
rcCompactHeightfield *chf = nullptr;
|
||||
rcContourSet *cset = nullptr;
|
||||
rcPolyMesh *poly_mesh = nullptr;
|
||||
rcPolyMeshDetail *detail_mesh = nullptr;
|
||||
rcContext ctx;
|
||||
|
||||
// added to keep track of steps, no functionality right now
|
||||
String bake_state = "";
|
||||
|
||||
bake_state = "Setting up Configuration..."; // step #1
|
||||
|
||||
const float *verts = source_geometry_vertices.ptr();
|
||||
const int nverts = source_geometry_vertices.size() / 3;
|
||||
const int *tris = source_geometry_indices.ptr();
|
||||
const int ntris = source_geometry_indices.size() / 3;
|
||||
|
||||
float bmin[3], bmax[3];
|
||||
rcCalcBounds(verts, nverts, bmin, bmax);
|
||||
|
||||
rcConfig cfg;
|
||||
memset(&cfg, 0, sizeof(cfg));
|
||||
|
||||
cfg.cs = p_navigation_mesh->get_cell_size();
|
||||
cfg.ch = p_navigation_mesh->get_cell_height();
|
||||
if (p_navigation_mesh->get_border_size() > 0.0) {
|
||||
cfg.borderSize = (int)Math::ceil(p_navigation_mesh->get_border_size() / cfg.cs);
|
||||
}
|
||||
cfg.walkableSlopeAngle = p_navigation_mesh->get_agent_max_slope();
|
||||
cfg.walkableHeight = (int)Math::ceil(p_navigation_mesh->get_agent_height() / cfg.ch);
|
||||
cfg.walkableClimb = (int)Math::floor(p_navigation_mesh->get_agent_max_climb() / cfg.ch);
|
||||
cfg.walkableRadius = (int)Math::ceil(p_navigation_mesh->get_agent_radius() / cfg.cs);
|
||||
cfg.maxEdgeLen = (int)(p_navigation_mesh->get_edge_max_length() / p_navigation_mesh->get_cell_size());
|
||||
cfg.maxSimplificationError = p_navigation_mesh->get_edge_max_error();
|
||||
cfg.minRegionArea = (int)(p_navigation_mesh->get_region_min_size() * p_navigation_mesh->get_region_min_size());
|
||||
cfg.mergeRegionArea = (int)(p_navigation_mesh->get_region_merge_size() * p_navigation_mesh->get_region_merge_size());
|
||||
cfg.maxVertsPerPoly = (int)p_navigation_mesh->get_vertices_per_polygon();
|
||||
cfg.detailSampleDist = MAX(p_navigation_mesh->get_cell_size() * p_navigation_mesh->get_detail_sample_distance(), 0.1f);
|
||||
cfg.detailSampleMaxError = p_navigation_mesh->get_cell_height() * p_navigation_mesh->get_detail_sample_max_error();
|
||||
|
||||
if (p_navigation_mesh->get_border_size() > 0.0 && Math::fmod(p_navigation_mesh->get_border_size(), p_navigation_mesh->get_cell_size()) != 0.0) {
|
||||
WARN_PRINT("Property border_size is ceiled to cell_size voxel units and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.walkableHeight * cfg.ch, p_navigation_mesh->get_agent_height())) {
|
||||
WARN_PRINT("Property agent_height is ceiled to cell_height voxel units and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.walkableClimb * cfg.ch, p_navigation_mesh->get_agent_max_climb())) {
|
||||
WARN_PRINT("Property agent_max_climb is floored to cell_height voxel units and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.walkableRadius * cfg.cs, p_navigation_mesh->get_agent_radius())) {
|
||||
WARN_PRINT("Property agent_radius is ceiled to cell_size voxel units and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.maxEdgeLen * cfg.cs, p_navigation_mesh->get_edge_max_length())) {
|
||||
WARN_PRINT("Property edge_max_length is rounded to cell_size voxel units and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.minRegionArea, p_navigation_mesh->get_region_min_size() * p_navigation_mesh->get_region_min_size())) {
|
||||
WARN_PRINT("Property region_min_size is converted to int and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.mergeRegionArea, p_navigation_mesh->get_region_merge_size() * p_navigation_mesh->get_region_merge_size())) {
|
||||
WARN_PRINT("Property region_merge_size is converted to int and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.maxVertsPerPoly, p_navigation_mesh->get_vertices_per_polygon())) {
|
||||
WARN_PRINT("Property vertices_per_polygon is converted to int and loses precision.");
|
||||
}
|
||||
if (p_navigation_mesh->get_cell_size() * p_navigation_mesh->get_detail_sample_distance() < 0.1f) {
|
||||
WARN_PRINT("Property detail_sample_distance is clamped to 0.1 world units as the resulting value from multiplying with cell_size is too low.");
|
||||
}
|
||||
|
||||
cfg.bmin[0] = bmin[0];
|
||||
cfg.bmin[1] = bmin[1];
|
||||
cfg.bmin[2] = bmin[2];
|
||||
cfg.bmax[0] = bmax[0];
|
||||
cfg.bmax[1] = bmax[1];
|
||||
cfg.bmax[2] = bmax[2];
|
||||
|
||||
AABB baking_aabb = p_navigation_mesh->get_filter_baking_aabb();
|
||||
if (baking_aabb.has_volume()) {
|
||||
Vector3 baking_aabb_offset = p_navigation_mesh->get_filter_baking_aabb_offset();
|
||||
cfg.bmin[0] = baking_aabb.position[0] + baking_aabb_offset.x;
|
||||
cfg.bmin[1] = baking_aabb.position[1] + baking_aabb_offset.y;
|
||||
cfg.bmin[2] = baking_aabb.position[2] + baking_aabb_offset.z;
|
||||
cfg.bmax[0] = cfg.bmin[0] + baking_aabb.size[0];
|
||||
cfg.bmax[1] = cfg.bmin[1] + baking_aabb.size[1];
|
||||
cfg.bmax[2] = cfg.bmin[2] + baking_aabb.size[2];
|
||||
}
|
||||
|
||||
bake_state = "Calculating grid size..."; // step #2
|
||||
rcCalcGridSize(cfg.bmin, cfg.bmax, cfg.cs, &cfg.width, &cfg.height);
|
||||
|
||||
// ~30000000 seems to be around sweetspot where Editor baking breaks
|
||||
if ((cfg.width * cfg.height) > 30000000 && GLOBAL_GET("navigation/baking/use_crash_prevention_checks")) {
|
||||
ERR_FAIL_MSG("Baking interrupted."
|
||||
"\nNavigationMesh baking process would likely crash the engine."
|
||||
"\nSource geometry is suspiciously big for the current Cell Size and Cell Height in the NavMesh Resource bake settings."
|
||||
"\nIf baking does not crash the engine or fail, the resulting NavigationMesh will create serious pathfinding performance issues."
|
||||
"\nIt is advised to increase Cell Size and/or Cell Height in the NavMesh Resource bake settings or reduce the size / scale of the source geometry."
|
||||
"\nIf you would like to try baking anyway, disable the 'navigation/baking/use_crash_prevention_checks' project setting.");
|
||||
return;
|
||||
}
|
||||
|
||||
bake_state = "Creating heightfield..."; // step #3
|
||||
hf = rcAllocHeightfield();
|
||||
|
||||
ERR_FAIL_NULL(hf);
|
||||
ERR_FAIL_COND(!rcCreateHeightfield(&ctx, *hf, cfg.width, cfg.height, cfg.bmin, cfg.bmax, cfg.cs, cfg.ch));
|
||||
|
||||
bake_state = "Marking walkable triangles..."; // step #4
|
||||
{
|
||||
Vector<unsigned char> tri_areas;
|
||||
tri_areas.resize(ntris);
|
||||
|
||||
ERR_FAIL_COND(tri_areas.is_empty());
|
||||
|
||||
memset(tri_areas.ptrw(), 0, ntris * sizeof(unsigned char));
|
||||
rcMarkWalkableTriangles(&ctx, cfg.walkableSlopeAngle, verts, nverts, tris, ntris, tri_areas.ptrw());
|
||||
|
||||
ERR_FAIL_COND(!rcRasterizeTriangles(&ctx, verts, nverts, tris, tri_areas.ptr(), ntris, *hf, cfg.walkableClimb));
|
||||
}
|
||||
|
||||
if (p_navigation_mesh->get_filter_low_hanging_obstacles()) {
|
||||
rcFilterLowHangingWalkableObstacles(&ctx, cfg.walkableClimb, *hf);
|
||||
}
|
||||
if (p_navigation_mesh->get_filter_ledge_spans()) {
|
||||
rcFilterLedgeSpans(&ctx, cfg.walkableHeight, cfg.walkableClimb, *hf);
|
||||
}
|
||||
if (p_navigation_mesh->get_filter_walkable_low_height_spans()) {
|
||||
rcFilterWalkableLowHeightSpans(&ctx, cfg.walkableHeight, *hf);
|
||||
}
|
||||
|
||||
bake_state = "Constructing compact heightfield..."; // step #5
|
||||
|
||||
chf = rcAllocCompactHeightfield();
|
||||
|
||||
ERR_FAIL_NULL(chf);
|
||||
ERR_FAIL_COND(!rcBuildCompactHeightfield(&ctx, cfg.walkableHeight, cfg.walkableClimb, *hf, *chf));
|
||||
|
||||
rcFreeHeightField(hf);
|
||||
hf = nullptr;
|
||||
|
||||
// Add obstacles to the source geometry. Those will be affected by e.g. agent_radius.
|
||||
if (!projected_obstructions.is_empty()) {
|
||||
for (const NavigationMeshSourceGeometryData3D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
|
||||
if (projected_obstruction.carve) {
|
||||
continue;
|
||||
}
|
||||
if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 3 != 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const float *projected_obstruction_verts = projected_obstruction.vertices.ptr();
|
||||
const int projected_obstruction_nverts = projected_obstruction.vertices.size() / 3;
|
||||
|
||||
rcMarkConvexPolyArea(&ctx, projected_obstruction_verts, projected_obstruction_nverts, projected_obstruction.elevation, projected_obstruction.elevation + projected_obstruction.height, RC_NULL_AREA, *chf);
|
||||
}
|
||||
}
|
||||
|
||||
bake_state = "Eroding walkable area..."; // step #6
|
||||
|
||||
ERR_FAIL_COND(!rcErodeWalkableArea(&ctx, cfg.walkableRadius, *chf));
|
||||
|
||||
// Carve obstacles to the eroded geometry. Those will NOT be affected by e.g. agent_radius because that step is already done.
|
||||
if (!projected_obstructions.is_empty()) {
|
||||
for (const NavigationMeshSourceGeometryData3D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
|
||||
if (!projected_obstruction.carve) {
|
||||
continue;
|
||||
}
|
||||
if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 3 != 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const float *projected_obstruction_verts = projected_obstruction.vertices.ptr();
|
||||
const int projected_obstruction_nverts = projected_obstruction.vertices.size() / 3;
|
||||
|
||||
rcMarkConvexPolyArea(&ctx, projected_obstruction_verts, projected_obstruction_nverts, projected_obstruction.elevation, projected_obstruction.elevation + projected_obstruction.height, RC_NULL_AREA, *chf);
|
||||
}
|
||||
}
|
||||
|
||||
bake_state = "Partitioning..."; // step #7
|
||||
|
||||
if (p_navigation_mesh->get_sample_partition_type() == NavigationMesh::SAMPLE_PARTITION_WATERSHED) {
|
||||
ERR_FAIL_COND(!rcBuildDistanceField(&ctx, *chf));
|
||||
ERR_FAIL_COND(!rcBuildRegions(&ctx, *chf, cfg.borderSize, cfg.minRegionArea, cfg.mergeRegionArea));
|
||||
} else if (p_navigation_mesh->get_sample_partition_type() == NavigationMesh::SAMPLE_PARTITION_MONOTONE) {
|
||||
ERR_FAIL_COND(!rcBuildRegionsMonotone(&ctx, *chf, cfg.borderSize, cfg.minRegionArea, cfg.mergeRegionArea));
|
||||
} else {
|
||||
ERR_FAIL_COND(!rcBuildLayerRegions(&ctx, *chf, cfg.borderSize, cfg.minRegionArea));
|
||||
}
|
||||
|
||||
bake_state = "Creating contours..."; // step #8
|
||||
|
||||
cset = rcAllocContourSet();
|
||||
|
||||
ERR_FAIL_NULL(cset);
|
||||
ERR_FAIL_COND(!rcBuildContours(&ctx, *chf, cfg.maxSimplificationError, cfg.maxEdgeLen, *cset));
|
||||
|
||||
bake_state = "Creating polymesh..."; // step #9
|
||||
|
||||
poly_mesh = rcAllocPolyMesh();
|
||||
ERR_FAIL_NULL(poly_mesh);
|
||||
ERR_FAIL_COND(!rcBuildPolyMesh(&ctx, *cset, cfg.maxVertsPerPoly, *poly_mesh));
|
||||
|
||||
detail_mesh = rcAllocPolyMeshDetail();
|
||||
ERR_FAIL_NULL(detail_mesh);
|
||||
ERR_FAIL_COND(!rcBuildPolyMeshDetail(&ctx, *poly_mesh, *chf, cfg.detailSampleDist, cfg.detailSampleMaxError, *detail_mesh));
|
||||
|
||||
rcFreeCompactHeightfield(chf);
|
||||
chf = nullptr;
|
||||
rcFreeContourSet(cset);
|
||||
cset = nullptr;
|
||||
|
||||
bake_state = "Converting to native navigation mesh..."; // step #10
|
||||
|
||||
Vector<Vector3> nav_vertices;
|
||||
Vector<Vector<int>> nav_polygons;
|
||||
|
||||
HashMap<Vector3, int> recast_vertex_to_native_index;
|
||||
LocalVector<int> recast_index_to_native_index;
|
||||
recast_index_to_native_index.resize(detail_mesh->nverts);
|
||||
|
||||
for (int i = 0; i < detail_mesh->nverts; i++) {
|
||||
const float *v = &detail_mesh->verts[i * 3];
|
||||
const Vector3 vertex = Vector3(v[0], v[1], v[2]);
|
||||
int *existing_index_ptr = recast_vertex_to_native_index.getptr(vertex);
|
||||
if (!existing_index_ptr) {
|
||||
int new_index = recast_vertex_to_native_index.size();
|
||||
recast_index_to_native_index[i] = new_index;
|
||||
recast_vertex_to_native_index[vertex] = new_index;
|
||||
nav_vertices.push_back(vertex);
|
||||
} else {
|
||||
recast_index_to_native_index[i] = *existing_index_ptr;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < detail_mesh->nmeshes; i++) {
|
||||
const unsigned int *detail_mesh_m = &detail_mesh->meshes[i * 4];
|
||||
const unsigned int detail_mesh_bverts = detail_mesh_m[0];
|
||||
const unsigned int detail_mesh_m_btris = detail_mesh_m[2];
|
||||
const unsigned int detail_mesh_ntris = detail_mesh_m[3];
|
||||
const unsigned char *detail_mesh_tris = &detail_mesh->tris[detail_mesh_m_btris * 4];
|
||||
for (unsigned int j = 0; j < detail_mesh_ntris; j++) {
|
||||
Vector<int> nav_indices;
|
||||
nav_indices.resize(3);
|
||||
// Polygon order in recast is opposite than godot's
|
||||
int index1 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 0]));
|
||||
int index2 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 2]));
|
||||
int index3 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 1]));
|
||||
|
||||
nav_indices.write[0] = recast_index_to_native_index[index1];
|
||||
nav_indices.write[1] = recast_index_to_native_index[index2];
|
||||
nav_indices.write[2] = recast_index_to_native_index[index3];
|
||||
|
||||
nav_polygons.push_back(nav_indices);
|
||||
}
|
||||
}
|
||||
|
||||
p_navigation_mesh->set_data(nav_vertices, nav_polygons);
|
||||
|
||||
bake_state = "Cleanup..."; // step #11
|
||||
|
||||
rcFreePolyMesh(poly_mesh);
|
||||
poly_mesh = nullptr;
|
||||
rcFreePolyMeshDetail(detail_mesh);
|
||||
detail_mesh = nullptr;
|
||||
|
||||
bake_state = "Baking finished."; // step #12
|
||||
}
|
||||
|
||||
bool NavMeshGenerator3D::generator_emit_callback(const Callable &p_callback) {
|
||||
ERR_FAIL_COND_V(!p_callback.is_valid(), false);
|
||||
|
||||
Callable::CallError ce;
|
||||
Variant result;
|
||||
p_callback.callp(nullptr, 0, result, ce);
|
||||
|
||||
return ce.error == Callable::CallError::CALL_OK;
|
||||
}
|
||||
|
||||
#endif // _3D_DISABLED
|
||||
106
engine/modules/navigation/3d/nav_mesh_generator_3d.h
Normal file
106
engine/modules/navigation/3d/nav_mesh_generator_3d.h
Normal file
|
|
@ -0,0 +1,106 @@
|
|||
/**************************************************************************/
|
||||
/* nav_mesh_generator_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_GENERATOR_3D_H
|
||||
#define NAV_MESH_GENERATOR_3D_H
|
||||
|
||||
#ifndef _3D_DISABLED
|
||||
|
||||
#include "core/object/class_db.h"
|
||||
#include "core/object/worker_thread_pool.h"
|
||||
#include "core/templates/rid_owner.h"
|
||||
#include "servers/navigation_server_3d.h"
|
||||
|
||||
class Node;
|
||||
class NavigationMesh;
|
||||
class NavigationMeshSourceGeometryData3D;
|
||||
|
||||
class NavMeshGenerator3D : public Object {
|
||||
static NavMeshGenerator3D *singleton;
|
||||
|
||||
static Mutex baking_navmesh_mutex;
|
||||
static Mutex generator_task_mutex;
|
||||
|
||||
static RWLock generator_parsers_rwlock;
|
||||
static LocalVector<NavMeshGeometryParser3D *> generator_parsers;
|
||||
|
||||
static bool use_threads;
|
||||
static bool baking_use_multiple_threads;
|
||||
static bool baking_use_high_priority_threads;
|
||||
|
||||
struct NavMeshGeneratorTask3D {
|
||||
enum TaskStatus {
|
||||
BAKING_STARTED,
|
||||
BAKING_FINISHED,
|
||||
BAKING_FAILED,
|
||||
CALLBACK_DISPATCHED,
|
||||
CALLBACK_FAILED,
|
||||
};
|
||||
|
||||
Ref<NavigationMesh> navigation_mesh;
|
||||
Ref<NavigationMeshSourceGeometryData3D> source_geometry_data;
|
||||
Callable callback;
|
||||
WorkerThreadPool::TaskID thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
|
||||
NavMeshGeneratorTask3D::TaskStatus status = NavMeshGeneratorTask3D::TaskStatus::BAKING_STARTED;
|
||||
};
|
||||
|
||||
static HashMap<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> generator_tasks;
|
||||
|
||||
static void generator_thread_bake(void *p_arg);
|
||||
|
||||
static HashSet<Ref<NavigationMesh>> baking_navmeshes;
|
||||
|
||||
static void generator_parse_geometry_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node, bool p_recurse_children);
|
||||
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 bool generator_emit_callback(const Callable &p_callback);
|
||||
|
||||
public:
|
||||
static NavMeshGenerator3D *get_singleton();
|
||||
|
||||
static void sync();
|
||||
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);
|
||||
|
||||
NavMeshGenerator3D();
|
||||
~NavMeshGenerator3D();
|
||||
};
|
||||
|
||||
#endif // _3D_DISABLED
|
||||
|
||||
#endif // NAV_MESH_GENERATOR_3D_H
|
||||
1130
engine/modules/navigation/3d/nav_mesh_queries_3d.cpp
Normal file
1130
engine/modules/navigation/3d/nav_mesh_queries_3d.cpp
Normal file
File diff suppressed because it is too large
Load diff
148
engine/modules/navigation/3d/nav_mesh_queries_3d.h
Normal file
148
engine/modules/navigation/3d/nav_mesh_queries_3d.h
Normal 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
51
engine/modules/navigation/3d/nav_region_iteration_3d.h
Normal 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
|
||||
78
engine/modules/navigation/3d/navigation_mesh_generator.cpp
Normal file
78
engine/modules/navigation/3d/navigation_mesh_generator.cpp
Normal file
|
|
@ -0,0 +1,78 @@
|
|||
/**************************************************************************/
|
||||
/* navigation_mesh_generator.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 "navigation_mesh_generator.h"
|
||||
|
||||
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
|
||||
#include "servers/navigation_server_3d.h"
|
||||
|
||||
NavigationMeshGenerator *NavigationMeshGenerator::singleton = nullptr;
|
||||
|
||||
NavigationMeshGenerator *NavigationMeshGenerator::get_singleton() {
|
||||
return singleton;
|
||||
}
|
||||
|
||||
NavigationMeshGenerator::NavigationMeshGenerator() {
|
||||
singleton = this;
|
||||
}
|
||||
|
||||
NavigationMeshGenerator::~NavigationMeshGenerator() {
|
||||
}
|
||||
|
||||
void NavigationMeshGenerator::bake(const Ref<NavigationMesh> &p_navigation_mesh, Node *p_root_node) {
|
||||
WARN_PRINT_ONCE("NavigationMeshGenerator::bake() is deprecated due to core threading changes. To upgrade existing code, first create a NavigationMeshSourceGeometryData3D resource. Use this resource with method parse_source_geometry_data() to parse the SceneTree for nodes that should contribute to the navigation mesh baking. The SceneTree parsing needs to happen on the main thread. After the parsing is finished use the resource with method bake_from_source_geometry_data() to bake a navigation mesh..");
|
||||
}
|
||||
|
||||
void NavigationMeshGenerator::clear(Ref<NavigationMesh> p_navigation_mesh) {
|
||||
if (p_navigation_mesh.is_valid()) {
|
||||
p_navigation_mesh->clear_polygons();
|
||||
p_navigation_mesh->set_vertices(Vector<Vector3>());
|
||||
}
|
||||
}
|
||||
|
||||
void NavigationMeshGenerator::parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
|
||||
NavigationServer3D::get_singleton()->parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node, p_callback);
|
||||
}
|
||||
|
||||
void NavigationMeshGenerator::bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback) {
|
||||
NavigationServer3D::get_singleton()->bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
|
||||
}
|
||||
|
||||
void NavigationMeshGenerator::_bind_methods() {
|
||||
ClassDB::bind_method(D_METHOD("bake", "navigation_mesh", "root_node"), &NavigationMeshGenerator::bake);
|
||||
ClassDB::bind_method(D_METHOD("clear", "navigation_mesh"), &NavigationMeshGenerator::clear);
|
||||
|
||||
ClassDB::bind_method(D_METHOD("parse_source_geometry_data", "navigation_mesh", "source_geometry_data", "root_node", "callback"), &NavigationMeshGenerator::parse_source_geometry_data, DEFVAL(Callable()));
|
||||
ClassDB::bind_method(D_METHOD("bake_from_source_geometry_data", "navigation_mesh", "source_geometry_data", "callback"), &NavigationMeshGenerator::bake_from_source_geometry_data, DEFVAL(Callable()));
|
||||
}
|
||||
|
||||
#endif
|
||||
64
engine/modules/navigation/3d/navigation_mesh_generator.h
Normal file
64
engine/modules/navigation/3d/navigation_mesh_generator.h
Normal file
|
|
@ -0,0 +1,64 @@
|
|||
/**************************************************************************/
|
||||
/* navigation_mesh_generator.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 NAVIGATION_MESH_GENERATOR_H
|
||||
#define NAVIGATION_MESH_GENERATOR_H
|
||||
|
||||
#ifndef _3D_DISABLED
|
||||
|
||||
#include "scene/3d/navigation_region_3d.h"
|
||||
#include "scene/resources/navigation_mesh.h"
|
||||
|
||||
class NavigationMeshSourceGeometryData3D;
|
||||
|
||||
class NavigationMeshGenerator : public Object {
|
||||
GDCLASS(NavigationMeshGenerator, Object);
|
||||
|
||||
static NavigationMeshGenerator *singleton;
|
||||
|
||||
protected:
|
||||
static void _bind_methods();
|
||||
|
||||
public:
|
||||
static NavigationMeshGenerator *get_singleton();
|
||||
|
||||
NavigationMeshGenerator();
|
||||
~NavigationMeshGenerator();
|
||||
|
||||
void bake(const Ref<NavigationMesh> &p_navigation_mesh, Node *p_root_node);
|
||||
void clear(Ref<NavigationMesh> p_navigation_mesh);
|
||||
|
||||
void parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
|
||||
void bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable());
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
#endif // NAVIGATION_MESH_GENERATOR_H
|
||||
86
engine/modules/navigation/SCsub
Normal file
86
engine/modules/navigation/SCsub
Normal file
|
|
@ -0,0 +1,86 @@
|
|||
#!/usr/bin/env python
|
||||
from misc.utility.scons_hints import *
|
||||
|
||||
Import("env")
|
||||
Import("env_modules")
|
||||
|
||||
env_navigation = env_modules.Clone()
|
||||
|
||||
# Thirdparty source files
|
||||
|
||||
thirdparty_obj = []
|
||||
|
||||
# Recast Thirdparty source files
|
||||
if env["builtin_recastnavigation"]:
|
||||
thirdparty_dir = "#thirdparty/recastnavigation/Recast/"
|
||||
thirdparty_sources = [
|
||||
"Source/Recast.cpp",
|
||||
"Source/RecastAlloc.cpp",
|
||||
"Source/RecastArea.cpp",
|
||||
"Source/RecastAssert.cpp",
|
||||
"Source/RecastContour.cpp",
|
||||
"Source/RecastFilter.cpp",
|
||||
"Source/RecastLayers.cpp",
|
||||
"Source/RecastMesh.cpp",
|
||||
"Source/RecastMeshDetail.cpp",
|
||||
"Source/RecastRasterization.cpp",
|
||||
"Source/RecastRegion.cpp",
|
||||
]
|
||||
thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
|
||||
|
||||
env_navigation.Prepend(CPPPATH=[thirdparty_dir + "Include"])
|
||||
|
||||
env_thirdparty = env_navigation.Clone()
|
||||
env_thirdparty.disable_warnings()
|
||||
env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
|
||||
|
||||
# RVO 2D Thirdparty source files
|
||||
if env["builtin_rvo2_2d"]:
|
||||
thirdparty_dir = "#thirdparty/rvo2/rvo2_2d/"
|
||||
thirdparty_sources = [
|
||||
"Agent2d.cpp",
|
||||
"Obstacle2d.cpp",
|
||||
"KdTree2d.cpp",
|
||||
"RVOSimulator2d.cpp",
|
||||
]
|
||||
thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
|
||||
|
||||
env_navigation.Prepend(CPPPATH=[thirdparty_dir])
|
||||
|
||||
env_thirdparty = env_navigation.Clone()
|
||||
env_thirdparty.disable_warnings()
|
||||
env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
|
||||
|
||||
# RVO 3D Thirdparty source files
|
||||
if env["builtin_rvo2_3d"]:
|
||||
thirdparty_dir = "#thirdparty/rvo2/rvo2_3d/"
|
||||
thirdparty_sources = [
|
||||
"Agent3d.cpp",
|
||||
"KdTree3d.cpp",
|
||||
"RVOSimulator3d.cpp",
|
||||
]
|
||||
thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
|
||||
|
||||
env_navigation.Prepend(CPPPATH=[thirdparty_dir])
|
||||
|
||||
env_thirdparty = env_navigation.Clone()
|
||||
env_thirdparty.disable_warnings()
|
||||
env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
|
||||
|
||||
|
||||
env.modules_sources += thirdparty_obj
|
||||
|
||||
# Godot source files
|
||||
|
||||
module_obj = []
|
||||
|
||||
env_navigation.add_source_files(module_obj, "*.cpp")
|
||||
env_navigation.add_source_files(module_obj, "2d/*.cpp")
|
||||
if not env["disable_3d"]:
|
||||
env_navigation.add_source_files(module_obj, "3d/*.cpp")
|
||||
if env.editor_build:
|
||||
env_navigation.add_source_files(module_obj, "editor/*.cpp")
|
||||
env.modules_sources += module_obj
|
||||
|
||||
# Needed to force rebuilding the module files when the thirdparty library is updated.
|
||||
env.Depends(module_obj, thirdparty_obj)
|
||||
7
engine/modules/navigation/config.py
Normal file
7
engine/modules/navigation/config.py
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
def can_build(env, platform):
|
||||
env.module_add_dependencies("navigation", ["csg", "gridmap"], True)
|
||||
return not env["disable_3d"]
|
||||
|
||||
|
||||
def configure(env):
|
||||
pass
|
||||
|
|
@ -0,0 +1,185 @@
|
|||
/**************************************************************************/
|
||||
/* navigation_mesh_editor_plugin.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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#include "navigation_mesh_editor_plugin.h"
|
||||
|
||||
#ifdef TOOLS_ENABLED
|
||||
|
||||
#include "editor/editor_node.h"
|
||||
#include "editor/editor_string_names.h"
|
||||
#include "scene/3d/navigation_region_3d.h"
|
||||
#include "scene/gui/box_container.h"
|
||||
#include "scene/gui/button.h"
|
||||
#include "scene/gui/dialogs.h"
|
||||
#include "scene/gui/label.h"
|
||||
|
||||
void NavigationMeshEditor::_node_removed(Node *p_node) {
|
||||
if (p_node == node) {
|
||||
node = nullptr;
|
||||
|
||||
hide();
|
||||
}
|
||||
}
|
||||
|
||||
void NavigationMeshEditor::_notification(int p_what) {
|
||||
switch (p_what) {
|
||||
case NOTIFICATION_ENTER_TREE: {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
void NavigationMeshEditor::_bake_pressed() {
|
||||
button_bake->set_pressed(false);
|
||||
|
||||
ERR_FAIL_NULL(node);
|
||||
Ref<NavigationMesh> navmesh = node->get_navigation_mesh();
|
||||
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;
|
||||
}
|
||||
|
||||
String path = navmesh->get_path();
|
||||
if (!path.is_resource_file()) {
|
||||
int srpos = path.find("::");
|
||||
if (srpos != -1) {
|
||||
String base = path.substr(0, srpos);
|
||||
if (ResourceLoader::get_resource_type(base) == "PackedScene") {
|
||||
if (!get_tree()->get_edited_scene_root() || get_tree()->get_edited_scene_root()->get_scene_file_path() != base) {
|
||||
err_dialog->set_text(TTR("Cannot generate navigation mesh because it does not belong to the edited scene. Make it unique first."));
|
||||
err_dialog->popup_centered();
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
if (FileAccess::exists(base + ".import")) {
|
||||
err_dialog->set_text(TTR("Cannot generate navigation mesh because it belongs to a resource which was imported."));
|
||||
err_dialog->popup_centered();
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (FileAccess::exists(path + ".import")) {
|
||||
err_dialog->set_text(TTR("Cannot generate navigation mesh because the resource was imported from another type."));
|
||||
err_dialog->popup_centered();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
node->bake_navigation_mesh(true);
|
||||
|
||||
node->update_gizmos();
|
||||
}
|
||||
|
||||
void NavigationMeshEditor::_clear_pressed() {
|
||||
if (node) {
|
||||
if (node->get_navigation_mesh().is_valid()) {
|
||||
node->get_navigation_mesh()->clear();
|
||||
}
|
||||
}
|
||||
|
||||
button_bake->set_pressed(false);
|
||||
bake_info->set_text("");
|
||||
|
||||
if (node) {
|
||||
node->update_gizmos();
|
||||
}
|
||||
}
|
||||
|
||||
void NavigationMeshEditor::edit(NavigationRegion3D *p_nav_region) {
|
||||
if (p_nav_region == nullptr || node == p_nav_region) {
|
||||
return;
|
||||
}
|
||||
|
||||
node = p_nav_region;
|
||||
}
|
||||
|
||||
NavigationMeshEditor::NavigationMeshEditor() {
|
||||
bake_hbox = memnew(HBoxContainer);
|
||||
|
||||
button_bake = memnew(Button);
|
||||
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"));
|
||||
button_bake->set_tooltip_text(TTR("Bakes the NavigationMesh by first parsing the scene for source geometry and then creating the navigation mesh vertices and polygons."));
|
||||
button_bake->connect(SceneStringName(pressed), callable_mp(this, &NavigationMeshEditor::_bake_pressed));
|
||||
|
||||
button_reset = memnew(Button);
|
||||
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."));
|
||||
button_reset->connect(SceneStringName(pressed), callable_mp(this, &NavigationMeshEditor::_clear_pressed));
|
||||
|
||||
bake_info = memnew(Label);
|
||||
bake_hbox->add_child(bake_info);
|
||||
|
||||
err_dialog = memnew(AcceptDialog);
|
||||
add_child(err_dialog);
|
||||
node = nullptr;
|
||||
}
|
||||
|
||||
NavigationMeshEditor::~NavigationMeshEditor() {
|
||||
}
|
||||
|
||||
void NavigationMeshEditorPlugin::edit(Object *p_object) {
|
||||
navigation_mesh_editor->edit(Object::cast_to<NavigationRegion3D>(p_object));
|
||||
}
|
||||
|
||||
bool NavigationMeshEditorPlugin::handles(Object *p_object) const {
|
||||
return p_object->is_class("NavigationRegion3D");
|
||||
}
|
||||
|
||||
void NavigationMeshEditorPlugin::make_visible(bool p_visible) {
|
||||
if (p_visible) {
|
||||
navigation_mesh_editor->show();
|
||||
navigation_mesh_editor->bake_hbox->show();
|
||||
} else {
|
||||
navigation_mesh_editor->hide();
|
||||
navigation_mesh_editor->bake_hbox->hide();
|
||||
navigation_mesh_editor->edit(nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
NavigationMeshEditorPlugin::NavigationMeshEditorPlugin() {
|
||||
navigation_mesh_editor = memnew(NavigationMeshEditor);
|
||||
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();
|
||||
}
|
||||
|
||||
NavigationMeshEditorPlugin::~NavigationMeshEditorPlugin() {
|
||||
}
|
||||
|
||||
#endif // TOOLS_ENABLED
|
||||
|
|
@ -0,0 +1,89 @@
|
|||
/**************************************************************************/
|
||||
/* navigation_mesh_editor_plugin.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 NAVIGATION_MESH_EDITOR_PLUGIN_H
|
||||
#define NAVIGATION_MESH_EDITOR_PLUGIN_H
|
||||
|
||||
#ifdef TOOLS_ENABLED
|
||||
|
||||
#include "editor/plugins/editor_plugin.h"
|
||||
|
||||
class AcceptDialog;
|
||||
class Button;
|
||||
class HBoxContainer;
|
||||
class Label;
|
||||
class NavigationRegion3D;
|
||||
|
||||
class NavigationMeshEditor : public Control {
|
||||
friend class NavigationMeshEditorPlugin;
|
||||
|
||||
GDCLASS(NavigationMeshEditor, Control);
|
||||
|
||||
AcceptDialog *err_dialog = nullptr;
|
||||
|
||||
HBoxContainer *bake_hbox = nullptr;
|
||||
Button *button_bake = nullptr;
|
||||
Button *button_reset = nullptr;
|
||||
Label *bake_info = nullptr;
|
||||
|
||||
NavigationRegion3D *node = nullptr;
|
||||
|
||||
void _bake_pressed();
|
||||
void _clear_pressed();
|
||||
|
||||
protected:
|
||||
void _node_removed(Node *p_node);
|
||||
void _notification(int p_what);
|
||||
|
||||
public:
|
||||
void edit(NavigationRegion3D *p_nav_region);
|
||||
NavigationMeshEditor();
|
||||
~NavigationMeshEditor();
|
||||
};
|
||||
|
||||
class NavigationMeshEditorPlugin : public EditorPlugin {
|
||||
GDCLASS(NavigationMeshEditorPlugin, EditorPlugin);
|
||||
|
||||
NavigationMeshEditor *navigation_mesh_editor = nullptr;
|
||||
|
||||
public:
|
||||
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;
|
||||
virtual void make_visible(bool p_visible) override;
|
||||
|
||||
NavigationMeshEditorPlugin();
|
||||
~NavigationMeshEditorPlugin();
|
||||
};
|
||||
|
||||
#endif // TOOLS_ENABLED
|
||||
|
||||
#endif // NAVIGATION_MESH_EDITOR_PLUGIN_H
|
||||
425
engine/modules/navigation/nav_agent.cpp
Normal file
425
engine/modules/navigation/nav_agent.cpp
Normal file
|
|
@ -0,0 +1,425 @@
|
|||
/**************************************************************************/
|
||||
/* nav_agent.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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#include "nav_agent.h"
|
||||
|
||||
#include "nav_map.h"
|
||||
|
||||
void NavAgent::set_avoidance_enabled(bool p_enabled) {
|
||||
avoidance_enabled = p_enabled;
|
||||
_update_rvo_agent_properties();
|
||||
}
|
||||
|
||||
void NavAgent::set_use_3d_avoidance(bool p_enabled) {
|
||||
use_3d_avoidance = p_enabled;
|
||||
_update_rvo_agent_properties();
|
||||
}
|
||||
|
||||
void NavAgent::_update_rvo_agent_properties() {
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.neighborDist_ = neighbor_distance;
|
||||
rvo_agent_3d.maxNeighbors_ = max_neighbors;
|
||||
rvo_agent_3d.timeHorizon_ = time_horizon_agents;
|
||||
rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles;
|
||||
rvo_agent_3d.radius_ = radius;
|
||||
rvo_agent_3d.maxSpeed_ = max_speed;
|
||||
rvo_agent_3d.position_ = RVO3D::Vector3(position.x, position.y, position.z);
|
||||
// Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing.
|
||||
//rvo_agent_3d.velocity_ = RVO3D::Vector3(velocity.x, velocity.y ,velocity.z);
|
||||
rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z);
|
||||
rvo_agent_3d.height_ = height;
|
||||
rvo_agent_3d.avoidance_layers_ = avoidance_layers;
|
||||
rvo_agent_3d.avoidance_mask_ = avoidance_mask;
|
||||
rvo_agent_3d.avoidance_priority_ = avoidance_priority;
|
||||
} else {
|
||||
rvo_agent_2d.neighborDist_ = neighbor_distance;
|
||||
rvo_agent_2d.maxNeighbors_ = max_neighbors;
|
||||
rvo_agent_2d.timeHorizon_ = time_horizon_agents;
|
||||
rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles;
|
||||
rvo_agent_2d.radius_ = radius;
|
||||
rvo_agent_2d.maxSpeed_ = max_speed;
|
||||
rvo_agent_2d.position_ = RVO2D::Vector2(position.x, position.z);
|
||||
rvo_agent_2d.elevation_ = position.y;
|
||||
// Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing.
|
||||
//rvo_agent_2d.velocity_ = RVO2D::Vector2(velocity.x, velocity.z);
|
||||
rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z);
|
||||
rvo_agent_2d.height_ = height;
|
||||
rvo_agent_2d.avoidance_layers_ = avoidance_layers;
|
||||
rvo_agent_2d.avoidance_mask_ = avoidance_mask;
|
||||
rvo_agent_2d.avoidance_priority_ = avoidance_priority;
|
||||
}
|
||||
|
||||
if (map != nullptr) {
|
||||
if (avoidance_enabled) {
|
||||
map->set_agent_as_controlled(this);
|
||||
} else {
|
||||
map->remove_agent_as_controlled(this);
|
||||
}
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_map(NavMap *p_map) {
|
||||
if (map == p_map) {
|
||||
return;
|
||||
}
|
||||
|
||||
cancel_sync_request();
|
||||
|
||||
if (map) {
|
||||
map->remove_agent(this);
|
||||
}
|
||||
|
||||
map = p_map;
|
||||
agent_dirty = true;
|
||||
|
||||
if (map) {
|
||||
map->add_agent(this);
|
||||
if (avoidance_enabled) {
|
||||
map->set_agent_as_controlled(this);
|
||||
}
|
||||
|
||||
request_sync();
|
||||
}
|
||||
}
|
||||
|
||||
bool NavAgent::is_map_changed() {
|
||||
if (map) {
|
||||
bool is_changed = map->get_iteration_id() != last_map_iteration_id;
|
||||
last_map_iteration_id = map->get_iteration_id();
|
||||
return is_changed;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void NavAgent::set_avoidance_callback(Callable p_callback) {
|
||||
avoidance_callback = p_callback;
|
||||
}
|
||||
|
||||
bool NavAgent::has_avoidance_callback() const {
|
||||
return avoidance_callback.is_valid();
|
||||
}
|
||||
|
||||
void NavAgent::dispatch_avoidance_callback() {
|
||||
if (!avoidance_callback.is_valid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3 new_velocity;
|
||||
|
||||
if (use_3d_avoidance) {
|
||||
new_velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
|
||||
} else {
|
||||
new_velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y());
|
||||
}
|
||||
|
||||
if (clamp_speed) {
|
||||
new_velocity = new_velocity.limit_length(max_speed);
|
||||
}
|
||||
|
||||
// Invoke the callback with the new velocity.
|
||||
avoidance_callback.call(new_velocity);
|
||||
}
|
||||
|
||||
void NavAgent::set_neighbor_distance(real_t p_neighbor_distance) {
|
||||
neighbor_distance = p_neighbor_distance;
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.neighborDist_ = neighbor_distance;
|
||||
} else {
|
||||
rvo_agent_2d.neighborDist_ = neighbor_distance;
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_max_neighbors(int p_max_neighbors) {
|
||||
max_neighbors = p_max_neighbors;
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.maxNeighbors_ = max_neighbors;
|
||||
} else {
|
||||
rvo_agent_2d.maxNeighbors_ = max_neighbors;
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_time_horizon_agents(real_t p_time_horizon) {
|
||||
time_horizon_agents = p_time_horizon;
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.timeHorizon_ = time_horizon_agents;
|
||||
} else {
|
||||
rvo_agent_2d.timeHorizon_ = time_horizon_agents;
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_time_horizon_obstacles(real_t p_time_horizon) {
|
||||
time_horizon_obstacles = p_time_horizon;
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles;
|
||||
} else {
|
||||
rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles;
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_radius(real_t p_radius) {
|
||||
radius = p_radius;
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.radius_ = radius;
|
||||
} else {
|
||||
rvo_agent_2d.radius_ = radius;
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_height(real_t p_height) {
|
||||
height = p_height;
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.height_ = height;
|
||||
} else {
|
||||
rvo_agent_2d.height_ = height;
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_max_speed(real_t p_max_speed) {
|
||||
max_speed = p_max_speed;
|
||||
if (avoidance_enabled) {
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.maxSpeed_ = max_speed;
|
||||
} else {
|
||||
rvo_agent_2d.maxSpeed_ = max_speed;
|
||||
}
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_position(const Vector3 p_position) {
|
||||
position = p_position;
|
||||
if (avoidance_enabled) {
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.position_ = RVO3D::Vector3(p_position.x, p_position.y, p_position.z);
|
||||
} else {
|
||||
rvo_agent_2d.elevation_ = p_position.y;
|
||||
rvo_agent_2d.position_ = RVO2D::Vector2(p_position.x, p_position.z);
|
||||
}
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_target_position(const Vector3 p_target_position) {
|
||||
target_position = p_target_position;
|
||||
}
|
||||
|
||||
void NavAgent::set_velocity(const Vector3 p_velocity) {
|
||||
// Sets the "wanted" velocity for an agent as a suggestion
|
||||
// This velocity is not guaranteed, RVO simulation will only try to fulfill it
|
||||
velocity = p_velocity;
|
||||
if (avoidance_enabled) {
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z);
|
||||
} else {
|
||||
rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z);
|
||||
}
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_velocity_forced(const Vector3 p_velocity) {
|
||||
// This function replaces the internal rvo simulation velocity
|
||||
// should only be used after the agent was teleported
|
||||
// as it destroys consistency in movement in cramped situations
|
||||
// use velocity instead to update with a safer "suggestion"
|
||||
velocity_forced = p_velocity;
|
||||
if (avoidance_enabled) {
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.velocity_ = RVO3D::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
|
||||
} else {
|
||||
rvo_agent_2d.velocity_ = RVO2D::Vector2(p_velocity.x, p_velocity.z);
|
||||
}
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::update() {
|
||||
// Updates this agent with the calculated results from the rvo simulation
|
||||
if (avoidance_enabled) {
|
||||
if (use_3d_avoidance) {
|
||||
velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
|
||||
} else {
|
||||
velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavAgent::set_avoidance_mask(uint32_t p_mask) {
|
||||
avoidance_mask = p_mask;
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.avoidance_mask_ = avoidance_mask;
|
||||
} else {
|
||||
rvo_agent_2d.avoidance_mask_ = avoidance_mask;
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_avoidance_layers(uint32_t p_layers) {
|
||||
avoidance_layers = p_layers;
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.avoidance_layers_ = avoidance_layers;
|
||||
} else {
|
||||
rvo_agent_2d.avoidance_layers_ = avoidance_layers;
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavAgent::set_avoidance_priority(real_t p_priority) {
|
||||
ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
|
||||
ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
|
||||
avoidance_priority = p_priority;
|
||||
if (use_3d_avoidance) {
|
||||
rvo_agent_3d.avoidance_priority_ = avoidance_priority;
|
||||
} else {
|
||||
rvo_agent_2d.avoidance_priority_ = avoidance_priority;
|
||||
}
|
||||
agent_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
bool NavAgent::is_dirty() const {
|
||||
return agent_dirty;
|
||||
}
|
||||
|
||||
void NavAgent::sync() {
|
||||
agent_dirty = false;
|
||||
}
|
||||
|
||||
const Dictionary NavAgent::get_avoidance_data() const {
|
||||
// Returns debug data from RVO simulation internals of this agent.
|
||||
Dictionary _avoidance_data;
|
||||
if (use_3d_avoidance) {
|
||||
_avoidance_data["max_neighbors"] = int(rvo_agent_3d.maxNeighbors_);
|
||||
_avoidance_data["max_speed"] = float(rvo_agent_3d.maxSpeed_);
|
||||
_avoidance_data["neighbor_distance"] = float(rvo_agent_3d.neighborDist_);
|
||||
_avoidance_data["new_velocity"] = Vector3(rvo_agent_3d.newVelocity_.x(), rvo_agent_3d.newVelocity_.y(), rvo_agent_3d.newVelocity_.z());
|
||||
_avoidance_data["velocity"] = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
|
||||
_avoidance_data["position"] = Vector3(rvo_agent_3d.position_.x(), rvo_agent_3d.position_.y(), rvo_agent_3d.position_.z());
|
||||
_avoidance_data["preferred_velocity"] = Vector3(rvo_agent_3d.prefVelocity_.x(), rvo_agent_3d.prefVelocity_.y(), rvo_agent_3d.prefVelocity_.z());
|
||||
_avoidance_data["radius"] = float(rvo_agent_3d.radius_);
|
||||
_avoidance_data["time_horizon_agents"] = float(rvo_agent_3d.timeHorizon_);
|
||||
_avoidance_data["time_horizon_obstacles"] = 0.0;
|
||||
_avoidance_data["height"] = float(rvo_agent_3d.height_);
|
||||
_avoidance_data["avoidance_layers"] = int(rvo_agent_3d.avoidance_layers_);
|
||||
_avoidance_data["avoidance_mask"] = int(rvo_agent_3d.avoidance_mask_);
|
||||
_avoidance_data["avoidance_priority"] = float(rvo_agent_3d.avoidance_priority_);
|
||||
} else {
|
||||
_avoidance_data["max_neighbors"] = int(rvo_agent_2d.maxNeighbors_);
|
||||
_avoidance_data["max_speed"] = float(rvo_agent_2d.maxSpeed_);
|
||||
_avoidance_data["neighbor_distance"] = float(rvo_agent_2d.neighborDist_);
|
||||
_avoidance_data["new_velocity"] = Vector3(rvo_agent_2d.newVelocity_.x(), 0.0, rvo_agent_2d.newVelocity_.y());
|
||||
_avoidance_data["velocity"] = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y());
|
||||
_avoidance_data["position"] = Vector3(rvo_agent_2d.position_.x(), 0.0, rvo_agent_2d.position_.y());
|
||||
_avoidance_data["preferred_velocity"] = Vector3(rvo_agent_2d.prefVelocity_.x(), 0.0, rvo_agent_2d.prefVelocity_.y());
|
||||
_avoidance_data["radius"] = float(rvo_agent_2d.radius_);
|
||||
_avoidance_data["time_horizon_agents"] = float(rvo_agent_2d.timeHorizon_);
|
||||
_avoidance_data["time_horizon_obstacles"] = float(rvo_agent_2d.timeHorizonObst_);
|
||||
_avoidance_data["height"] = float(rvo_agent_2d.height_);
|
||||
_avoidance_data["avoidance_layers"] = int(rvo_agent_2d.avoidance_layers_);
|
||||
_avoidance_data["avoidance_mask"] = int(rvo_agent_2d.avoidance_mask_);
|
||||
_avoidance_data["avoidance_priority"] = float(rvo_agent_2d.avoidance_priority_);
|
||||
}
|
||||
return _avoidance_data;
|
||||
}
|
||||
|
||||
void NavAgent::set_paused(bool p_paused) {
|
||||
if (paused == p_paused) {
|
||||
return;
|
||||
}
|
||||
|
||||
paused = p_paused;
|
||||
|
||||
if (map) {
|
||||
if (paused) {
|
||||
map->remove_agent_as_controlled(this);
|
||||
} else {
|
||||
map->set_agent_as_controlled(this);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
162
engine/modules/navigation/nav_agent.h
Normal file
162
engine/modules/navigation/nav_agent.h
Normal file
|
|
@ -0,0 +1,162 @@
|
|||
/**************************************************************************/
|
||||
/* nav_agent.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_AGENT_H
|
||||
#define NAV_AGENT_H
|
||||
|
||||
#include "nav_rid.h"
|
||||
|
||||
#include "core/object/class_db.h"
|
||||
#include "core/templates/self_list.h"
|
||||
|
||||
#include <Agent2d.h>
|
||||
#include <Agent3d.h>
|
||||
|
||||
class NavMap;
|
||||
|
||||
class NavAgent : public NavRid {
|
||||
Vector3 position;
|
||||
Vector3 target_position;
|
||||
Vector3 velocity;
|
||||
Vector3 velocity_forced;
|
||||
real_t height = 1.0;
|
||||
real_t radius = 1.0;
|
||||
real_t max_speed = 1.0;
|
||||
real_t time_horizon_agents = 1.0;
|
||||
real_t time_horizon_obstacles = 0.0;
|
||||
int max_neighbors = 5;
|
||||
real_t neighbor_distance = 5.0;
|
||||
Vector3 safe_velocity;
|
||||
bool clamp_speed = true; // Experimental, clamps velocity to max_speed.
|
||||
|
||||
NavMap *map = nullptr;
|
||||
|
||||
RVO2D::Agent2D rvo_agent_2d;
|
||||
RVO3D::Agent3D rvo_agent_3d;
|
||||
bool use_3d_avoidance = false;
|
||||
bool avoidance_enabled = false;
|
||||
|
||||
uint32_t avoidance_layers = 1;
|
||||
uint32_t avoidance_mask = 1;
|
||||
real_t avoidance_priority = 1.0;
|
||||
|
||||
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; }
|
||||
|
||||
void set_use_3d_avoidance(bool p_enabled);
|
||||
bool get_use_3d_avoidance() { return use_3d_avoidance; }
|
||||
|
||||
void set_map(NavMap *p_map);
|
||||
NavMap *get_map() { return map; }
|
||||
|
||||
bool is_map_changed();
|
||||
|
||||
RVO2D::Agent2D *get_rvo_agent_2d() { return &rvo_agent_2d; }
|
||||
RVO3D::Agent3D *get_rvo_agent_3d() { return &rvo_agent_3d; }
|
||||
|
||||
void set_avoidance_callback(Callable p_callback);
|
||||
bool has_avoidance_callback() const;
|
||||
|
||||
void dispatch_avoidance_callback();
|
||||
|
||||
void set_neighbor_distance(real_t p_neighbor_distance);
|
||||
real_t get_neighbor_distance() const { return neighbor_distance; }
|
||||
|
||||
void set_max_neighbors(int p_max_neighbors);
|
||||
int get_max_neighbors() const { return max_neighbors; }
|
||||
|
||||
void set_time_horizon_agents(real_t p_time_horizon);
|
||||
real_t get_time_horizon_agents() const { return time_horizon_agents; }
|
||||
|
||||
void set_time_horizon_obstacles(real_t p_time_horizon);
|
||||
real_t get_time_horizon_obstacles() const { return time_horizon_obstacles; }
|
||||
|
||||
void set_radius(real_t p_radius);
|
||||
real_t get_radius() const { return radius; }
|
||||
|
||||
void set_height(real_t p_height);
|
||||
real_t get_height() const { return height; }
|
||||
|
||||
void set_max_speed(real_t p_max_speed);
|
||||
real_t get_max_speed() const { return max_speed; }
|
||||
|
||||
void set_position(const Vector3 p_position);
|
||||
const Vector3 &get_position() const { return position; }
|
||||
|
||||
void set_target_position(const Vector3 p_target_position);
|
||||
const Vector3 &get_target_position() const { return target_position; }
|
||||
|
||||
void set_velocity(const Vector3 p_velocity);
|
||||
const Vector3 &get_velocity() const { return velocity; }
|
||||
|
||||
void set_velocity_forced(const Vector3 p_velocity);
|
||||
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; }
|
||||
|
||||
void set_avoidance_mask(uint32_t p_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; }
|
||||
|
||||
void set_paused(bool p_paused);
|
||||
bool get_paused() const;
|
||||
|
||||
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();
|
||||
|
||||
// RVO debug data from the last frame update.
|
||||
const Dictionary get_avoidance_data() const;
|
||||
|
||||
private:
|
||||
void _update_rvo_agent_properties();
|
||||
};
|
||||
|
||||
#endif // NAV_AGENT_H
|
||||
70
engine/modules/navigation/nav_base.h
Normal file
70
engine/modules/navigation/nav_base.h
Normal file
|
|
@ -0,0 +1,70 @@
|
|||
/**************************************************************************/
|
||||
/* nav_base.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_H
|
||||
#define NAV_BASE_H
|
||||
|
||||
#include "nav_rid.h"
|
||||
#include "nav_utils.h"
|
||||
|
||||
#include "servers/navigation/navigation_utilities.h"
|
||||
|
||||
class NavMap;
|
||||
|
||||
class NavBase : public NavRid {
|
||||
protected:
|
||||
uint32_t navigation_layers = 1;
|
||||
real_t enter_cost = 0.0;
|
||||
real_t travel_cost = 1.0;
|
||||
ObjectID owner_id;
|
||||
NavigationUtilities::PathSegmentType type;
|
||||
|
||||
public:
|
||||
NavigationUtilities::PathSegmentType get_type() const { return type; }
|
||||
|
||||
virtual void set_use_edge_connections(bool p_enabled) {}
|
||||
virtual bool get_use_edge_connections() const { return false; }
|
||||
|
||||
virtual void set_navigation_layers(uint32_t p_navigation_layers) {}
|
||||
uint32_t get_navigation_layers() const { return navigation_layers; }
|
||||
|
||||
virtual void set_enter_cost(real_t p_enter_cost) {}
|
||||
real_t get_enter_cost() const { return enter_cost; }
|
||||
|
||||
virtual void set_travel_cost(real_t p_travel_cost) {}
|
||||
real_t get_travel_cost() const { return travel_cost; }
|
||||
|
||||
virtual void set_owner_id(ObjectID p_owner_id) {}
|
||||
ObjectID get_owner_id() const { return owner_id; }
|
||||
|
||||
virtual ~NavBase() {}
|
||||
};
|
||||
|
||||
#endif // NAV_BASE_H
|
||||
180
engine/modules/navigation/nav_link.cpp
Normal file
180
engine/modules/navigation/nav_link.cpp
Normal file
|
|
@ -0,0 +1,180 @@
|
|||
/**************************************************************************/
|
||||
/* nav_link.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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#include "nav_link.h"
|
||||
|
||||
#include "nav_map.h"
|
||||
|
||||
void NavLink::set_map(NavMap *p_map) {
|
||||
if (map == p_map) {
|
||||
return;
|
||||
}
|
||||
|
||||
cancel_sync_request();
|
||||
|
||||
if (map) {
|
||||
map->remove_link(this);
|
||||
}
|
||||
|
||||
map = p_map;
|
||||
link_dirty = true;
|
||||
|
||||
if (map) {
|
||||
map->add_link(this);
|
||||
request_sync();
|
||||
}
|
||||
}
|
||||
|
||||
void NavLink::set_enabled(bool p_enabled) {
|
||||
if (enabled == p_enabled) {
|
||||
return;
|
||||
}
|
||||
enabled = 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) {
|
||||
return;
|
||||
}
|
||||
bidirectional = p_bidirectional;
|
||||
link_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavLink::set_start_position(const Vector3 p_position) {
|
||||
if (start_position == p_position) {
|
||||
return;
|
||||
}
|
||||
start_position = p_position;
|
||||
link_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavLink::set_end_position(const Vector3 p_position) {
|
||||
if (end_position == p_position) {
|
||||
return;
|
||||
}
|
||||
end_position = p_position;
|
||||
link_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
103
engine/modules/navigation/nav_link.h
Normal file
103
engine/modules/navigation/nav_link.h
Normal file
|
|
@ -0,0 +1,103 @@
|
|||
/**************************************************************************/
|
||||
/* nav_link.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_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;
|
||||
Vector3 start_position;
|
||||
Vector3 end_position;
|
||||
bool enabled = true;
|
||||
|
||||
bool link_dirty = true;
|
||||
|
||||
SelfList<NavLink> sync_dirty_request_list_element;
|
||||
|
||||
public:
|
||||
NavLink();
|
||||
~NavLink();
|
||||
|
||||
void set_map(NavMap *p_map);
|
||||
NavMap *get_map() const {
|
||||
return map;
|
||||
}
|
||||
|
||||
void set_enabled(bool p_enabled);
|
||||
bool get_enabled() const { return enabled; }
|
||||
|
||||
void set_bidirectional(bool p_bidirectional);
|
||||
bool is_bidirectional() const {
|
||||
return bidirectional;
|
||||
}
|
||||
|
||||
void set_start_position(Vector3 p_position);
|
||||
Vector3 get_start_position() const {
|
||||
return start_position;
|
||||
}
|
||||
|
||||
void set_end_position(Vector3 p_position);
|
||||
Vector3 get_end_position() const {
|
||||
return end_position;
|
||||
}
|
||||
|
||||
// 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
|
||||
882
engine/modules/navigation/nav_map.cpp
Normal file
882
engine/modules/navigation/nav_map.cpp
Normal file
|
|
@ -0,0 +1,882 @@
|
|||
/**************************************************************************/
|
||||
/* nav_map.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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#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"
|
||||
#include "nav_agent.h"
|
||||
#include "nav_link.h"
|
||||
#include "nav_obstacle.h"
|
||||
#include "nav_region.h"
|
||||
|
||||
#include "core/config/project_settings.h"
|
||||
#include "core/object/worker_thread_pool.h"
|
||||
|
||||
#include <Obstacle2d.h>
|
||||
|
||||
#ifdef DEBUG_ENABLED
|
||||
#define NAVMAP_ITERATION_ZERO_ERROR_MSG() \
|
||||
ERR_PRINT_ONCE("NavigationServer navigation map query failed because it was made before first map synchronization.\n\
|
||||
NavigationServer 'map_changed' signal can be used to receive update notifications.\n\
|
||||
NavigationServer 'map_get_iteration_id()' can be used to check if a map has finished its newest iteration.");
|
||||
#else
|
||||
#define NAVMAP_ITERATION_ZERO_ERROR_MSG()
|
||||
#endif // DEBUG_ENABLED
|
||||
|
||||
#define GET_MAP_ITERATION() \
|
||||
iteration_slot_rwlock.read_lock(); \
|
||||
NavMapIteration &map_iteration = iteration_slots[iteration_slot_index]; \
|
||||
NavMapIterationRead iteration_read_lock(map_iteration); \
|
||||
iteration_slot_rwlock.read_unlock();
|
||||
|
||||
#define GET_MAP_ITERATION_CONST() \
|
||||
iteration_slot_rwlock.read_lock(); \
|
||||
const NavMapIteration &map_iteration = iteration_slots[iteration_slot_index]; \
|
||||
NavMapIterationRead iteration_read_lock(map_iteration); \
|
||||
iteration_slot_rwlock.read_unlock();
|
||||
|
||||
void NavMap::set_up(Vector3 p_up) {
|
||||
if (up == p_up) {
|
||||
return;
|
||||
}
|
||||
up = p_up;
|
||||
map_settings_dirty = true;
|
||||
}
|
||||
|
||||
void NavMap::set_cell_size(real_t p_cell_size) {
|
||||
if (cell_size == p_cell_size) {
|
||||
return;
|
||||
}
|
||||
cell_size = MAX(p_cell_size, NavigationDefaults3D::navmesh_cell_size_min);
|
||||
_update_merge_rasterizer_cell_dimensions();
|
||||
map_settings_dirty = true;
|
||||
}
|
||||
|
||||
void NavMap::set_cell_height(real_t p_cell_height) {
|
||||
if (cell_height == p_cell_height) {
|
||||
return;
|
||||
}
|
||||
cell_height = MAX(p_cell_height, NavigationDefaults3D::navmesh_cell_size_min);
|
||||
_update_merge_rasterizer_cell_dimensions();
|
||||
map_settings_dirty = true;
|
||||
}
|
||||
|
||||
void NavMap::set_merge_rasterizer_cell_scale(float p_value) {
|
||||
if (merge_rasterizer_cell_scale == p_value) {
|
||||
return;
|
||||
}
|
||||
merge_rasterizer_cell_scale = MAX(p_value, NavigationDefaults3D::navmesh_cell_size_min);
|
||||
_update_merge_rasterizer_cell_dimensions();
|
||||
map_settings_dirty = true;
|
||||
}
|
||||
|
||||
void NavMap::set_use_edge_connections(bool p_enabled) {
|
||||
if (use_edge_connections == p_enabled) {
|
||||
return;
|
||||
}
|
||||
use_edge_connections = p_enabled;
|
||||
iteration_dirty = true;
|
||||
}
|
||||
|
||||
void NavMap::set_edge_connection_margin(real_t p_edge_connection_margin) {
|
||||
if (edge_connection_margin == p_edge_connection_margin) {
|
||||
return;
|
||||
}
|
||||
edge_connection_margin = p_edge_connection_margin;
|
||||
iteration_dirty = true;
|
||||
}
|
||||
|
||||
void NavMap::set_link_connection_radius(real_t p_link_connection_radius) {
|
||||
if (link_connection_radius == p_link_connection_radius) {
|
||||
return;
|
||||
}
|
||||
link_connection_radius = p_link_connection_radius;
|
||||
iteration_dirty = true;
|
||||
}
|
||||
|
||||
const Vector3 &NavMap::get_merge_rasterizer_cell_size() const {
|
||||
return merge_rasterizer_cell_size;
|
||||
}
|
||||
|
||||
gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
|
||||
const int x = static_cast<int>(Math::floor(p_pos.x / merge_rasterizer_cell_size.x));
|
||||
const int y = static_cast<int>(Math::floor(p_pos.y / merge_rasterizer_cell_size.y));
|
||||
const int z = static_cast<int>(Math::floor(p_pos.z / merge_rasterizer_cell_size.z));
|
||||
|
||||
gd::PointKey p;
|
||||
p.key = 0;
|
||||
p.x = x;
|
||||
p.y = y;
|
||||
p.z = z;
|
||||
return p;
|
||||
}
|
||||
|
||||
void NavMap::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task) {
|
||||
if (iteration_id == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
GET_MAP_ITERATION();
|
||||
|
||||
map_iteration.path_query_slots_semaphore.wait();
|
||||
|
||||
map_iteration.path_query_slots_mutex.lock();
|
||||
for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : map_iteration.path_query_slots) {
|
||||
if (!p_path_query_slot.in_use) {
|
||||
p_path_query_slot.in_use = true;
|
||||
p_query_task.path_query_slot = &p_path_query_slot;
|
||||
break;
|
||||
}
|
||||
}
|
||||
map_iteration.path_query_slots_mutex.unlock();
|
||||
|
||||
if (p_query_task.path_query_slot == nullptr) {
|
||||
map_iteration.path_query_slots_semaphore.post();
|
||||
ERR_FAIL_NULL_MSG(p_query_task.path_query_slot, "No unused NavMap path query slot found! This should never happen :(.");
|
||||
}
|
||||
|
||||
p_query_task.map_up = map_iteration.map_up;
|
||||
|
||||
NavMeshQueries3D::query_task_map_iteration_get_path(p_query_task, map_iteration);
|
||||
|
||||
map_iteration.path_query_slots_mutex.lock();
|
||||
uint32_t used_slot_index = p_query_task.path_query_slot->slot_index;
|
||||
map_iteration.path_query_slots[used_slot_index].in_use = false;
|
||||
p_query_task.path_query_slot = nullptr;
|
||||
map_iteration.path_query_slots_mutex.unlock();
|
||||
|
||||
map_iteration.path_query_slots_semaphore.post();
|
||||
}
|
||||
|
||||
Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
|
||||
if (iteration_id == 0) {
|
||||
NAVMAP_ITERATION_ZERO_ERROR_MSG();
|
||||
return Vector3();
|
||||
}
|
||||
|
||||
GET_MAP_ITERATION_CONST();
|
||||
|
||||
return NavMeshQueries3D::map_iteration_get_closest_point_to_segment(map_iteration, p_from, p_to, p_use_collision);
|
||||
}
|
||||
|
||||
Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
|
||||
if (iteration_id == 0) {
|
||||
NAVMAP_ITERATION_ZERO_ERROR_MSG();
|
||||
return Vector3();
|
||||
}
|
||||
|
||||
GET_MAP_ITERATION_CONST();
|
||||
|
||||
return NavMeshQueries3D::map_iteration_get_closest_point(map_iteration, p_point);
|
||||
}
|
||||
|
||||
Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
|
||||
if (iteration_id == 0) {
|
||||
NAVMAP_ITERATION_ZERO_ERROR_MSG();
|
||||
return Vector3();
|
||||
}
|
||||
|
||||
GET_MAP_ITERATION_CONST();
|
||||
|
||||
return NavMeshQueries3D::map_iteration_get_closest_point_normal(map_iteration, p_point);
|
||||
}
|
||||
|
||||
RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
|
||||
if (iteration_id == 0) {
|
||||
NAVMAP_ITERATION_ZERO_ERROR_MSG();
|
||||
return RID();
|
||||
}
|
||||
|
||||
GET_MAP_ITERATION_CONST();
|
||||
|
||||
return NavMeshQueries3D::map_iteration_get_closest_point_owner(map_iteration, p_point);
|
||||
}
|
||||
|
||||
gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
|
||||
GET_MAP_ITERATION_CONST();
|
||||
|
||||
return NavMeshQueries3D::map_iteration_get_closest_point_info(map_iteration, p_point);
|
||||
}
|
||||
|
||||
void NavMap::add_region(NavRegion *p_region) {
|
||||
regions.push_back(p_region);
|
||||
iteration_dirty = true;
|
||||
}
|
||||
|
||||
void NavMap::remove_region(NavRegion *p_region) {
|
||||
int64_t region_index = regions.find(p_region);
|
||||
if (region_index >= 0) {
|
||||
regions.remove_at_unordered(region_index);
|
||||
iteration_dirty = true;
|
||||
}
|
||||
}
|
||||
|
||||
void NavMap::add_link(NavLink *p_link) {
|
||||
links.push_back(p_link);
|
||||
iteration_dirty = true;
|
||||
}
|
||||
|
||||
void NavMap::remove_link(NavLink *p_link) {
|
||||
int64_t link_index = links.find(p_link);
|
||||
if (link_index >= 0) {
|
||||
links.remove_at_unordered(link_index);
|
||||
iteration_dirty = true;
|
||||
}
|
||||
}
|
||||
|
||||
bool NavMap::has_agent(NavAgent *agent) const {
|
||||
return agents.has(agent);
|
||||
}
|
||||
|
||||
void NavMap::add_agent(NavAgent *agent) {
|
||||
if (!has_agent(agent)) {
|
||||
agents.push_back(agent);
|
||||
agents_dirty = true;
|
||||
}
|
||||
}
|
||||
|
||||
void NavMap::remove_agent(NavAgent *agent) {
|
||||
remove_agent_as_controlled(agent);
|
||||
int64_t agent_index = agents.find(agent);
|
||||
if (agent_index >= 0) {
|
||||
agents.remove_at_unordered(agent_index);
|
||||
agents_dirty = true;
|
||||
}
|
||||
}
|
||||
|
||||
bool NavMap::has_obstacle(NavObstacle *obstacle) const {
|
||||
return obstacles.has(obstacle);
|
||||
}
|
||||
|
||||
void NavMap::add_obstacle(NavObstacle *obstacle) {
|
||||
if (obstacle->get_paused()) {
|
||||
// No point in adding a paused obstacle, it will add itself when unpaused again.
|
||||
return;
|
||||
}
|
||||
|
||||
if (!has_obstacle(obstacle)) {
|
||||
obstacles.push_back(obstacle);
|
||||
obstacles_dirty = true;
|
||||
}
|
||||
}
|
||||
|
||||
void NavMap::remove_obstacle(NavObstacle *obstacle) {
|
||||
int64_t obstacle_index = obstacles.find(obstacle);
|
||||
if (obstacle_index >= 0) {
|
||||
obstacles.remove_at_unordered(obstacle_index);
|
||||
obstacles_dirty = true;
|
||||
}
|
||||
}
|
||||
|
||||
void NavMap::set_agent_as_controlled(NavAgent *agent) {
|
||||
remove_agent_as_controlled(agent);
|
||||
|
||||
if (agent->get_paused()) {
|
||||
// No point in adding a paused agent, it will add itself when unpaused again.
|
||||
return;
|
||||
}
|
||||
|
||||
if (agent->get_use_3d_avoidance()) {
|
||||
int64_t agent_3d_index = active_3d_avoidance_agents.find(agent);
|
||||
if (agent_3d_index < 0) {
|
||||
active_3d_avoidance_agents.push_back(agent);
|
||||
agents_dirty = true;
|
||||
}
|
||||
} else {
|
||||
int64_t agent_2d_index = active_2d_avoidance_agents.find(agent);
|
||||
if (agent_2d_index < 0) {
|
||||
active_2d_avoidance_agents.push_back(agent);
|
||||
agents_dirty = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavMap::remove_agent_as_controlled(NavAgent *agent) {
|
||||
int64_t agent_3d_index = active_3d_avoidance_agents.find(agent);
|
||||
if (agent_3d_index >= 0) {
|
||||
active_3d_avoidance_agents.remove_at_unordered(agent_3d_index);
|
||||
agents_dirty = true;
|
||||
}
|
||||
int64_t agent_2d_index = active_2d_avoidance_agents.find(agent);
|
||||
if (agent_2d_index >= 0) {
|
||||
active_2d_avoidance_agents.remove_at_unordered(agent_2d_index);
|
||||
agents_dirty = true;
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 NavMap::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
|
||||
GET_MAP_ITERATION_CONST();
|
||||
|
||||
return NavMeshQueries3D::map_iteration_get_random_point(map_iteration, p_navigation_layers, p_uniformly);
|
||||
}
|
||||
|
||||
void NavMap::_build_iteration() {
|
||||
if (!iteration_dirty || iteration_building || iteration_ready) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Get the next free iteration slot that should be potentially unused.
|
||||
iteration_slot_rwlock.read_lock();
|
||||
NavMapIteration &next_map_iteration = iteration_slots[(iteration_slot_index + 1) % 2];
|
||||
// Check if the iteration slot is truly free or still used by an external thread.
|
||||
bool iteration_is_free = next_map_iteration.users.get() == 0;
|
||||
iteration_slot_rwlock.read_unlock();
|
||||
|
||||
if (!iteration_is_free) {
|
||||
// A long running pathfinding thread or something is still reading
|
||||
// from this older iteration and needs to finish first.
|
||||
// Return and wait for the next sync cycle to check again.
|
||||
return;
|
||||
}
|
||||
|
||||
// Iteration slot is free and no longer used by anything, let's build.
|
||||
|
||||
iteration_dirty = false;
|
||||
iteration_building = true;
|
||||
iteration_ready = false;
|
||||
|
||||
// We don't need to hold any lock because at this point nothing else can touch it.
|
||||
// All new queries are already forwarded to the other iteration slot.
|
||||
|
||||
iteration_build.reset();
|
||||
|
||||
iteration_build.merge_rasterizer_cell_size = get_merge_rasterizer_cell_size();
|
||||
iteration_build.use_edge_connections = get_use_edge_connections();
|
||||
iteration_build.edge_connection_margin = get_edge_connection_margin();
|
||||
iteration_build.link_connection_radius = get_link_connection_radius();
|
||||
|
||||
uint32_t enabled_region_count = 0;
|
||||
uint32_t enabled_link_count = 0;
|
||||
|
||||
for (NavRegion *region : regions) {
|
||||
if (!region->get_enabled()) {
|
||||
continue;
|
||||
}
|
||||
enabled_region_count++;
|
||||
}
|
||||
for (NavLink *link : links) {
|
||||
if (!link->get_enabled()) {
|
||||
continue;
|
||||
}
|
||||
enabled_link_count++;
|
||||
}
|
||||
|
||||
next_map_iteration.region_ptr_to_region_id.clear();
|
||||
|
||||
next_map_iteration.region_iterations.clear();
|
||||
next_map_iteration.link_iterations.clear();
|
||||
|
||||
next_map_iteration.region_iterations.resize(enabled_region_count);
|
||||
next_map_iteration.link_iterations.resize(enabled_link_count);
|
||||
|
||||
uint32_t region_id_count = 0;
|
||||
uint32_t link_id_count = 0;
|
||||
|
||||
for (NavRegion *region : regions) {
|
||||
if (!region->get_enabled()) {
|
||||
continue;
|
||||
}
|
||||
NavRegionIteration ®ion_iteration = next_map_iteration.region_iterations[region_id_count];
|
||||
region_iteration.id = region_id_count++;
|
||||
region->get_iteration_update(region_iteration);
|
||||
next_map_iteration.region_ptr_to_region_id[region] = (uint32_t)region_iteration.id;
|
||||
}
|
||||
for (NavLink *link : links) {
|
||||
if (!link->get_enabled()) {
|
||||
continue;
|
||||
}
|
||||
NavLinkIteration &link_iteration = next_map_iteration.link_iterations[link_id_count];
|
||||
link_iteration.id = link_id_count++;
|
||||
link->get_iteration_update(link_iteration);
|
||||
}
|
||||
|
||||
next_map_iteration.map_up = get_up();
|
||||
|
||||
iteration_build.map_iteration = &next_map_iteration;
|
||||
|
||||
if (use_async_iterations) {
|
||||
iteration_build_thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMap::_build_iteration_threaded, &iteration_build, true, SNAME("NavMapBuilder3D"));
|
||||
} else {
|
||||
NavMapBuilder3D::build_navmap_iteration(iteration_build);
|
||||
|
||||
iteration_building = false;
|
||||
iteration_ready = true;
|
||||
}
|
||||
}
|
||||
|
||||
void NavMap::_build_iteration_threaded(void *p_arg) {
|
||||
NavMapIterationBuild *_iteration_build = static_cast<NavMapIterationBuild *>(p_arg);
|
||||
|
||||
NavMapBuilder3D::build_navmap_iteration(*_iteration_build);
|
||||
}
|
||||
|
||||
void NavMap::_sync_iteration() {
|
||||
if (iteration_building || !iteration_ready) {
|
||||
return;
|
||||
}
|
||||
|
||||
performance_data.pm_polygon_count = iteration_build.performance_data.pm_polygon_count;
|
||||
performance_data.pm_edge_count = iteration_build.performance_data.pm_edge_count;
|
||||
performance_data.pm_edge_merge_count = iteration_build.performance_data.pm_edge_merge_count;
|
||||
performance_data.pm_edge_connection_count = iteration_build.performance_data.pm_edge_connection_count;
|
||||
performance_data.pm_edge_free_count = iteration_build.performance_data.pm_edge_free_count;
|
||||
|
||||
iteration_id = iteration_id % UINT32_MAX + 1;
|
||||
|
||||
// Finally ping-pong switch the iteration slot.
|
||||
iteration_slot_rwlock.write_lock();
|
||||
uint32_t next_iteration_slot_index = (iteration_slot_index + 1) % 2;
|
||||
iteration_slot_index = next_iteration_slot_index;
|
||||
iteration_slot_rwlock.write_unlock();
|
||||
|
||||
iteration_ready = false;
|
||||
}
|
||||
|
||||
void NavMap::sync() {
|
||||
// Performance Monitor.
|
||||
performance_data.pm_region_count = regions.size();
|
||||
performance_data.pm_agent_count = agents.size();
|
||||
performance_data.pm_link_count = links.size();
|
||||
performance_data.pm_obstacle_count = obstacles.size();
|
||||
|
||||
_sync_dirty_map_update_requests();
|
||||
|
||||
if (iteration_dirty && !iteration_building && !iteration_ready) {
|
||||
_build_iteration();
|
||||
}
|
||||
if (use_async_iterations && iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) {
|
||||
if (WorkerThreadPool::get_singleton()->is_task_completed(iteration_build_thread_task_id)) {
|
||||
WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id);
|
||||
|
||||
iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
|
||||
iteration_building = false;
|
||||
iteration_ready = true;
|
||||
}
|
||||
}
|
||||
if (iteration_ready) {
|
||||
_sync_iteration();
|
||||
}
|
||||
|
||||
map_settings_dirty = false;
|
||||
|
||||
_sync_avoidance();
|
||||
}
|
||||
|
||||
void NavMap::_sync_avoidance() {
|
||||
_sync_dirty_avoidance_update_requests();
|
||||
|
||||
if (obstacles_dirty || agents_dirty) {
|
||||
_update_rvo_simulation();
|
||||
}
|
||||
|
||||
obstacles_dirty = false;
|
||||
agents_dirty = false;
|
||||
}
|
||||
|
||||
void NavMap::_update_rvo_obstacles_tree_2d() {
|
||||
int obstacle_vertex_count = 0;
|
||||
for (NavObstacle *obstacle : obstacles) {
|
||||
obstacle_vertex_count += obstacle->get_vertices().size();
|
||||
}
|
||||
|
||||
// Cleaning old obstacles.
|
||||
for (size_t i = 0; i < rvo_simulation_2d.obstacles_.size(); ++i) {
|
||||
delete rvo_simulation_2d.obstacles_[i];
|
||||
}
|
||||
rvo_simulation_2d.obstacles_.clear();
|
||||
|
||||
// Cannot use LocalVector here as RVO library expects std::vector to build KdTree
|
||||
std::vector<RVO2D::Obstacle2D *> &raw_obstacles = rvo_simulation_2d.obstacles_;
|
||||
raw_obstacles.reserve(obstacle_vertex_count);
|
||||
|
||||
// The following block is modified copy from RVO2D::AddObstacle()
|
||||
// Obstacles are linked and depend on all other obstacles.
|
||||
for (NavObstacle *obstacle : obstacles) {
|
||||
const Vector3 &_obstacle_position = obstacle->get_position();
|
||||
const Vector<Vector3> &_obstacle_vertices = obstacle->get_vertices();
|
||||
|
||||
if (_obstacle_vertices.size() < 2) {
|
||||
continue;
|
||||
}
|
||||
|
||||
std::vector<RVO2D::Vector2> rvo_2d_vertices;
|
||||
rvo_2d_vertices.reserve(_obstacle_vertices.size());
|
||||
|
||||
uint32_t _obstacle_avoidance_layers = obstacle->get_avoidance_layers();
|
||||
real_t _obstacle_height = obstacle->get_height();
|
||||
|
||||
for (const Vector3 &_obstacle_vertex : _obstacle_vertices) {
|
||||
#ifdef TOOLS_ENABLED
|
||||
if (_obstacle_vertex.y != 0) {
|
||||
WARN_PRINT_ONCE("Y coordinates of static obstacle vertices are ignored. Please use obstacle position Y to change elevation of obstacle.");
|
||||
}
|
||||
#endif
|
||||
rvo_2d_vertices.push_back(RVO2D::Vector2(_obstacle_vertex.x + _obstacle_position.x, _obstacle_vertex.z + _obstacle_position.z));
|
||||
}
|
||||
|
||||
const size_t obstacleNo = raw_obstacles.size();
|
||||
|
||||
for (size_t i = 0; i < rvo_2d_vertices.size(); i++) {
|
||||
RVO2D::Obstacle2D *rvo_2d_obstacle = new RVO2D::Obstacle2D();
|
||||
rvo_2d_obstacle->point_ = rvo_2d_vertices[i];
|
||||
rvo_2d_obstacle->height_ = _obstacle_height;
|
||||
rvo_2d_obstacle->elevation_ = _obstacle_position.y;
|
||||
|
||||
rvo_2d_obstacle->avoidance_layers_ = _obstacle_avoidance_layers;
|
||||
|
||||
if (i != 0) {
|
||||
rvo_2d_obstacle->prevObstacle_ = raw_obstacles.back();
|
||||
rvo_2d_obstacle->prevObstacle_->nextObstacle_ = rvo_2d_obstacle;
|
||||
}
|
||||
|
||||
if (i == rvo_2d_vertices.size() - 1) {
|
||||
rvo_2d_obstacle->nextObstacle_ = raw_obstacles[obstacleNo];
|
||||
rvo_2d_obstacle->nextObstacle_->prevObstacle_ = rvo_2d_obstacle;
|
||||
}
|
||||
|
||||
rvo_2d_obstacle->unitDir_ = normalize(rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)] - rvo_2d_vertices[i]);
|
||||
|
||||
if (rvo_2d_vertices.size() == 2) {
|
||||
rvo_2d_obstacle->isConvex_ = true;
|
||||
} else {
|
||||
rvo_2d_obstacle->isConvex_ = (leftOf(rvo_2d_vertices[(i == 0 ? rvo_2d_vertices.size() - 1 : i - 1)], rvo_2d_vertices[i], rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f);
|
||||
}
|
||||
|
||||
rvo_2d_obstacle->id_ = raw_obstacles.size();
|
||||
|
||||
raw_obstacles.push_back(rvo_2d_obstacle);
|
||||
}
|
||||
}
|
||||
|
||||
rvo_simulation_2d.kdTree_->buildObstacleTree(raw_obstacles);
|
||||
}
|
||||
|
||||
void NavMap::_update_rvo_agents_tree_2d() {
|
||||
// Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
|
||||
std::vector<RVO2D::Agent2D *> raw_agents;
|
||||
raw_agents.reserve(active_2d_avoidance_agents.size());
|
||||
for (NavAgent *agent : active_2d_avoidance_agents) {
|
||||
raw_agents.push_back(agent->get_rvo_agent_2d());
|
||||
}
|
||||
rvo_simulation_2d.kdTree_->buildAgentTree(raw_agents);
|
||||
}
|
||||
|
||||
void NavMap::_update_rvo_agents_tree_3d() {
|
||||
// Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
|
||||
std::vector<RVO3D::Agent3D *> raw_agents;
|
||||
raw_agents.reserve(active_3d_avoidance_agents.size());
|
||||
for (NavAgent *agent : active_3d_avoidance_agents) {
|
||||
raw_agents.push_back(agent->get_rvo_agent_3d());
|
||||
}
|
||||
rvo_simulation_3d.kdTree_->buildAgentTree(raw_agents);
|
||||
}
|
||||
|
||||
void NavMap::_update_rvo_simulation() {
|
||||
if (obstacles_dirty) {
|
||||
_update_rvo_obstacles_tree_2d();
|
||||
}
|
||||
if (agents_dirty) {
|
||||
_update_rvo_agents_tree_2d();
|
||||
_update_rvo_agents_tree_3d();
|
||||
}
|
||||
}
|
||||
|
||||
void NavMap::compute_single_avoidance_step_2d(uint32_t index, NavAgent **agent) {
|
||||
(*(agent + index))->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
|
||||
(*(agent + index))->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
|
||||
(*(agent + index))->get_rvo_agent_2d()->update(&rvo_simulation_2d);
|
||||
(*(agent + index))->update();
|
||||
}
|
||||
|
||||
void NavMap::compute_single_avoidance_step_3d(uint32_t index, NavAgent **agent) {
|
||||
(*(agent + index))->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d);
|
||||
(*(agent + index))->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d);
|
||||
(*(agent + index))->get_rvo_agent_3d()->update(&rvo_simulation_3d);
|
||||
(*(agent + index))->update();
|
||||
}
|
||||
|
||||
void NavMap::step(real_t p_deltatime) {
|
||||
deltatime = p_deltatime;
|
||||
|
||||
rvo_simulation_2d.setTimeStep(float(deltatime));
|
||||
rvo_simulation_3d.setTimeStep(float(deltatime));
|
||||
|
||||
if (active_2d_avoidance_agents.size() > 0) {
|
||||
if (use_threads && avoidance_use_multiple_threads) {
|
||||
WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_avoidance_step_2d, active_2d_avoidance_agents.ptr(), active_2d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents2D"));
|
||||
WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
|
||||
} else {
|
||||
for (NavAgent *agent : active_2d_avoidance_agents) {
|
||||
agent->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
|
||||
agent->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
|
||||
agent->get_rvo_agent_2d()->update(&rvo_simulation_2d);
|
||||
agent->update();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (active_3d_avoidance_agents.size() > 0) {
|
||||
if (use_threads && avoidance_use_multiple_threads) {
|
||||
WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_avoidance_step_3d, active_3d_avoidance_agents.ptr(), active_3d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents3D"));
|
||||
WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
|
||||
} else {
|
||||
for (NavAgent *agent : active_3d_avoidance_agents) {
|
||||
agent->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d);
|
||||
agent->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d);
|
||||
agent->get_rvo_agent_3d()->update(&rvo_simulation_3d);
|
||||
agent->update();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavMap::dispatch_callbacks() {
|
||||
for (NavAgent *agent : active_2d_avoidance_agents) {
|
||||
agent->dispatch_avoidance_callback();
|
||||
}
|
||||
|
||||
for (NavAgent *agent : active_3d_avoidance_agents) {
|
||||
agent->dispatch_avoidance_callback();
|
||||
}
|
||||
}
|
||||
|
||||
void NavMap::_update_merge_rasterizer_cell_dimensions() {
|
||||
merge_rasterizer_cell_size.x = cell_size * merge_rasterizer_cell_scale;
|
||||
merge_rasterizer_cell_size.y = cell_height * merge_rasterizer_cell_scale;
|
||||
merge_rasterizer_cell_size.z = cell_size * merge_rasterizer_cell_scale;
|
||||
}
|
||||
|
||||
int NavMap::get_region_connections_count(NavRegion *p_region) const {
|
||||
ERR_FAIL_NULL_V(p_region, 0);
|
||||
|
||||
GET_MAP_ITERATION_CONST();
|
||||
|
||||
HashMap<NavRegion *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
|
||||
if (found_id) {
|
||||
HashMap<uint32_t, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
|
||||
if (found_connections) {
|
||||
return found_connections->value.size();
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
Vector3 NavMap::get_region_connection_pathway_start(NavRegion *p_region, int p_connection_id) const {
|
||||
ERR_FAIL_NULL_V(p_region, Vector3());
|
||||
|
||||
GET_MAP_ITERATION_CONST();
|
||||
|
||||
HashMap<NavRegion *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
|
||||
if (found_id) {
|
||||
HashMap<uint32_t, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
|
||||
if (found_connections) {
|
||||
ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3());
|
||||
return found_connections->value[p_connection_id].pathway_start;
|
||||
}
|
||||
}
|
||||
|
||||
return Vector3();
|
||||
}
|
||||
|
||||
Vector3 NavMap::get_region_connection_pathway_end(NavRegion *p_region, int p_connection_id) const {
|
||||
ERR_FAIL_NULL_V(p_region, Vector3());
|
||||
|
||||
GET_MAP_ITERATION_CONST();
|
||||
|
||||
HashMap<NavRegion *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
|
||||
if (found_id) {
|
||||
HashMap<uint32_t, LocalVector<gd::Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
|
||||
if (found_connections) {
|
||||
ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector3());
|
||||
return found_connections->value[p_connection_id].pathway_end;
|
||||
}
|
||||
}
|
||||
|
||||
return Vector3();
|
||||
}
|
||||
|
||||
void NavMap::add_region_sync_dirty_request(SelfList<NavRegion> *p_sync_request) {
|
||||
if (p_sync_request->in_list()) {
|
||||
return;
|
||||
}
|
||||
sync_dirty_requests.regions.add(p_sync_request);
|
||||
}
|
||||
|
||||
void NavMap::add_link_sync_dirty_request(SelfList<NavLink> *p_sync_request) {
|
||||
if (p_sync_request->in_list()) {
|
||||
return;
|
||||
}
|
||||
sync_dirty_requests.links.add(p_sync_request);
|
||||
}
|
||||
|
||||
void NavMap::add_agent_sync_dirty_request(SelfList<NavAgent> *p_sync_request) {
|
||||
if (p_sync_request->in_list()) {
|
||||
return;
|
||||
}
|
||||
sync_dirty_requests.agents.add(p_sync_request);
|
||||
}
|
||||
|
||||
void NavMap::add_obstacle_sync_dirty_request(SelfList<NavObstacle> *p_sync_request) {
|
||||
if (p_sync_request->in_list()) {
|
||||
return;
|
||||
}
|
||||
sync_dirty_requests.obstacles.add(p_sync_request);
|
||||
}
|
||||
|
||||
void NavMap::remove_region_sync_dirty_request(SelfList<NavRegion> *p_sync_request) {
|
||||
if (!p_sync_request->in_list()) {
|
||||
return;
|
||||
}
|
||||
sync_dirty_requests.regions.remove(p_sync_request);
|
||||
}
|
||||
|
||||
void NavMap::remove_link_sync_dirty_request(SelfList<NavLink> *p_sync_request) {
|
||||
if (!p_sync_request->in_list()) {
|
||||
return;
|
||||
}
|
||||
sync_dirty_requests.links.remove(p_sync_request);
|
||||
}
|
||||
|
||||
void NavMap::remove_agent_sync_dirty_request(SelfList<NavAgent> *p_sync_request) {
|
||||
if (!p_sync_request->in_list()) {
|
||||
return;
|
||||
}
|
||||
sync_dirty_requests.agents.remove(p_sync_request);
|
||||
}
|
||||
|
||||
void NavMap::remove_obstacle_sync_dirty_request(SelfList<NavObstacle> *p_sync_request) {
|
||||
if (!p_sync_request->in_list()) {
|
||||
return;
|
||||
}
|
||||
sync_dirty_requests.obstacles.remove(p_sync_request);
|
||||
}
|
||||
|
||||
void NavMap::_sync_dirty_map_update_requests() {
|
||||
// If entire map settings changed make all regions dirty.
|
||||
if (map_settings_dirty) {
|
||||
for (NavRegion *region : regions) {
|
||||
region->scratch_polygons();
|
||||
}
|
||||
iteration_dirty = true;
|
||||
}
|
||||
|
||||
if (!iteration_dirty) {
|
||||
iteration_dirty = sync_dirty_requests.regions.first() || sync_dirty_requests.links.first();
|
||||
}
|
||||
|
||||
// Sync NavRegions.
|
||||
for (SelfList<NavRegion> *element = sync_dirty_requests.regions.first(); element; element = element->next()) {
|
||||
element->self()->sync();
|
||||
}
|
||||
sync_dirty_requests.regions.clear();
|
||||
|
||||
// Sync NavLinks.
|
||||
for (SelfList<NavLink> *element = sync_dirty_requests.links.first(); element; element = element->next()) {
|
||||
element->self()->sync();
|
||||
}
|
||||
sync_dirty_requests.links.clear();
|
||||
}
|
||||
|
||||
void NavMap::_sync_dirty_avoidance_update_requests() {
|
||||
// Sync NavAgents.
|
||||
if (!agents_dirty) {
|
||||
agents_dirty = sync_dirty_requests.agents.first();
|
||||
}
|
||||
for (SelfList<NavAgent> *element = sync_dirty_requests.agents.first(); element; element = element->next()) {
|
||||
element->self()->sync();
|
||||
}
|
||||
sync_dirty_requests.agents.clear();
|
||||
|
||||
// Sync NavObstacles.
|
||||
if (!obstacles_dirty) {
|
||||
obstacles_dirty = sync_dirty_requests.obstacles.first();
|
||||
}
|
||||
for (SelfList<NavObstacle> *element = sync_dirty_requests.obstacles.first(); element; element = element->next()) {
|
||||
element->self()->sync();
|
||||
}
|
||||
sync_dirty_requests.obstacles.clear();
|
||||
}
|
||||
|
||||
void NavMap::set_use_async_iterations(bool p_enabled) {
|
||||
if (use_async_iterations == p_enabled) {
|
||||
return;
|
||||
}
|
||||
#ifdef THREADS_ENABLED
|
||||
use_async_iterations = p_enabled;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool NavMap::get_use_async_iterations() const {
|
||||
return use_async_iterations;
|
||||
}
|
||||
|
||||
NavMap::NavMap() {
|
||||
avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads");
|
||||
avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads");
|
||||
|
||||
path_query_slots_max = GLOBAL_GET("navigation/pathfinding/max_threads");
|
||||
|
||||
int processor_count = OS::get_singleton()->get_processor_count();
|
||||
if (path_query_slots_max < 0) {
|
||||
path_query_slots_max = processor_count;
|
||||
}
|
||||
if (processor_count < path_query_slots_max) {
|
||||
path_query_slots_max = processor_count;
|
||||
}
|
||||
if (path_query_slots_max < 1) {
|
||||
path_query_slots_max = 1;
|
||||
}
|
||||
|
||||
iteration_slots.resize(2);
|
||||
|
||||
for (NavMapIteration &iteration_slot : iteration_slots) {
|
||||
iteration_slot.path_query_slots.resize(path_query_slots_max);
|
||||
for (uint32_t i = 0; i < iteration_slot.path_query_slots.size(); i++) {
|
||||
iteration_slot.path_query_slots[i].slot_index = i;
|
||||
}
|
||||
iteration_slot.path_query_slots_semaphore.post(path_query_slots_max);
|
||||
}
|
||||
|
||||
#ifdef THREADS_ENABLED
|
||||
use_async_iterations = GLOBAL_GET("navigation/world/map_use_async_iterations");
|
||||
#else
|
||||
use_async_iterations = false;
|
||||
#endif
|
||||
}
|
||||
|
||||
NavMap::~NavMap() {
|
||||
if (iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) {
|
||||
WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id);
|
||||
iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
|
||||
}
|
||||
}
|
||||
274
engine/modules/navigation/nav_map.h
Normal file
274
engine/modules/navigation/nav_map.h
Normal file
|
|
@ -0,0 +1,274 @@
|
|||
/**************************************************************************/
|
||||
/* nav_map.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_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>
|
||||
#include <RVOSimulator2d.h>
|
||||
#include <RVOSimulator3d.h>
|
||||
|
||||
class NavLink;
|
||||
class NavRegion;
|
||||
class NavAgent;
|
||||
class NavObstacle;
|
||||
|
||||
class NavMap : public NavRid {
|
||||
/// 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 = NavigationDefaults3D::navmesh_cell_size;
|
||||
real_t cell_height = NavigationDefaults3D::navmesh_cell_height;
|
||||
|
||||
// For the inter-region merging to work, internal rasterization is performed.
|
||||
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 = 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 = NavigationDefaults3D::link_connection_radius;
|
||||
|
||||
bool map_settings_dirty = true;
|
||||
|
||||
/// Map regions
|
||||
LocalVector<NavRegion *> regions;
|
||||
|
||||
/// Map links
|
||||
LocalVector<NavLink *> links;
|
||||
|
||||
/// RVO avoidance worlds
|
||||
RVO2D::RVOSimulator2D rvo_simulation_2d;
|
||||
RVO3D::RVOSimulator3D rvo_simulation_3d;
|
||||
|
||||
/// avoidance controlled agents
|
||||
LocalVector<NavAgent *> active_2d_avoidance_agents;
|
||||
LocalVector<NavAgent *> active_3d_avoidance_agents;
|
||||
|
||||
/// dirty flag when one of the agent's arrays are modified
|
||||
bool agents_dirty = true;
|
||||
|
||||
/// All the Agents (even the controlled one)
|
||||
LocalVector<NavAgent *> agents;
|
||||
|
||||
/// All the avoidance obstacles (both static and dynamic)
|
||||
LocalVector<NavObstacle *> obstacles;
|
||||
|
||||
/// Are rvo obstacles modified?
|
||||
bool obstacles_dirty = true;
|
||||
|
||||
/// Physics delta time
|
||||
real_t deltatime = 0.0;
|
||||
|
||||
/// Change the id each time the map is updated.
|
||||
uint32_t iteration_id = 0;
|
||||
|
||||
bool use_threads = true;
|
||||
bool avoidance_use_multiple_threads = true;
|
||||
bool avoidance_use_high_priority_threads = true;
|
||||
|
||||
// Performance Monitor
|
||||
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();
|
||||
~NavMap();
|
||||
|
||||
uint32_t get_iteration_id() const { return iteration_id; }
|
||||
|
||||
void set_up(Vector3 p_up);
|
||||
Vector3 get_up() const {
|
||||
return up;
|
||||
}
|
||||
|
||||
void set_cell_size(real_t p_cell_size);
|
||||
real_t get_cell_size() const {
|
||||
return cell_size;
|
||||
}
|
||||
|
||||
void set_cell_height(real_t p_cell_height);
|
||||
real_t get_cell_height() const { return cell_height; }
|
||||
|
||||
void set_merge_rasterizer_cell_scale(float p_value);
|
||||
float get_merge_rasterizer_cell_scale() const {
|
||||
return merge_rasterizer_cell_scale;
|
||||
}
|
||||
|
||||
void set_use_edge_connections(bool p_enabled);
|
||||
bool get_use_edge_connections() const {
|
||||
return use_edge_connections;
|
||||
}
|
||||
|
||||
void set_edge_connection_margin(real_t p_edge_connection_margin);
|
||||
real_t get_edge_connection_margin() const {
|
||||
return edge_connection_margin;
|
||||
}
|
||||
|
||||
void set_link_connection_radius(real_t p_link_connection_radius);
|
||||
real_t get_link_connection_radius() const {
|
||||
return link_connection_radius;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
|
||||
Vector3 get_closest_point(const Vector3 &p_point) const;
|
||||
Vector3 get_closest_point_normal(const Vector3 &p_point) const;
|
||||
gd::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
|
||||
RID get_closest_point_owner(const Vector3 &p_point) const;
|
||||
|
||||
void add_region(NavRegion *p_region);
|
||||
void remove_region(NavRegion *p_region);
|
||||
const LocalVector<NavRegion *> &get_regions() const {
|
||||
return regions;
|
||||
}
|
||||
|
||||
void add_link(NavLink *p_link);
|
||||
void remove_link(NavLink *p_link);
|
||||
const LocalVector<NavLink *> &get_links() const {
|
||||
return links;
|
||||
}
|
||||
|
||||
bool has_agent(NavAgent *agent) const;
|
||||
void add_agent(NavAgent *agent);
|
||||
void remove_agent(NavAgent *agent);
|
||||
const LocalVector<NavAgent *> &get_agents() const {
|
||||
return agents;
|
||||
}
|
||||
|
||||
void set_agent_as_controlled(NavAgent *agent);
|
||||
void remove_agent_as_controlled(NavAgent *agent);
|
||||
|
||||
bool has_obstacle(NavObstacle *obstacle) const;
|
||||
void add_obstacle(NavObstacle *obstacle);
|
||||
void remove_obstacle(NavObstacle *obstacle);
|
||||
const LocalVector<NavObstacle *> &get_obstacles() const {
|
||||
return obstacles;
|
||||
}
|
||||
|
||||
Vector3 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const;
|
||||
|
||||
void sync();
|
||||
void step(real_t p_deltatime);
|
||||
void dispatch_callbacks();
|
||||
|
||||
// Performance Monitor
|
||||
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 _sync_avoidance();
|
||||
void _update_rvo_simulation();
|
||||
void _update_rvo_obstacles_tree_2d();
|
||||
void _update_rvo_agents_tree_2d();
|
||||
void _update_rvo_agents_tree_3d();
|
||||
|
||||
void _update_merge_rasterizer_cell_dimensions();
|
||||
};
|
||||
|
||||
#endif // NAV_MAP_H
|
||||
254
engine/modules/navigation/nav_obstacle.cpp
Normal file
254
engine/modules/navigation/nav_obstacle.cpp
Normal file
|
|
@ -0,0 +1,254 @@
|
|||
/**************************************************************************/
|
||||
/* nav_obstacle.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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#include "nav_obstacle.h"
|
||||
|
||||
#include "nav_agent.h"
|
||||
#include "nav_map.h"
|
||||
|
||||
void NavObstacle::set_agent(NavAgent *p_agent) {
|
||||
if (agent == p_agent) {
|
||||
return;
|
||||
}
|
||||
|
||||
agent = p_agent;
|
||||
|
||||
internal_update_agent();
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavObstacle::set_avoidance_enabled(bool p_enabled) {
|
||||
if (avoidance_enabled == p_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
avoidance_enabled = p_enabled;
|
||||
obstacle_dirty = true;
|
||||
|
||||
internal_update_agent();
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavObstacle::set_use_3d_avoidance(bool p_enabled) {
|
||||
if (use_3d_avoidance == p_enabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
use_3d_avoidance = p_enabled;
|
||||
obstacle_dirty = true;
|
||||
|
||||
if (agent) {
|
||||
agent->set_use_3d_avoidance(use_3d_avoidance);
|
||||
}
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavObstacle::set_map(NavMap *p_map) {
|
||||
if (map == p_map) {
|
||||
return;
|
||||
}
|
||||
|
||||
cancel_sync_request();
|
||||
|
||||
if (map) {
|
||||
map->remove_obstacle(this);
|
||||
if (agent) {
|
||||
agent->set_map(nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
map = p_map;
|
||||
obstacle_dirty = true;
|
||||
|
||||
if (map) {
|
||||
map->add_obstacle(this);
|
||||
internal_update_agent();
|
||||
|
||||
request_sync();
|
||||
}
|
||||
}
|
||||
|
||||
void NavObstacle::set_position(const Vector3 p_position) {
|
||||
if (position == p_position) {
|
||||
return;
|
||||
}
|
||||
|
||||
position = p_position;
|
||||
obstacle_dirty = true;
|
||||
|
||||
if (agent) {
|
||||
agent->set_position(position);
|
||||
}
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavObstacle::set_radius(real_t p_radius) {
|
||||
if (radius == p_radius) {
|
||||
return;
|
||||
}
|
||||
|
||||
radius = p_radius;
|
||||
|
||||
if (agent) {
|
||||
agent->set_radius(radius);
|
||||
}
|
||||
}
|
||||
|
||||
void NavObstacle::set_height(const real_t p_height) {
|
||||
if (height == p_height) {
|
||||
return;
|
||||
}
|
||||
|
||||
height = p_height;
|
||||
obstacle_dirty = true;
|
||||
|
||||
if (agent) {
|
||||
agent->set_height(height);
|
||||
}
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
void NavObstacle::set_velocity(const Vector3 p_velocity) {
|
||||
velocity = p_velocity;
|
||||
|
||||
if (agent) {
|
||||
agent->set_velocity(velocity);
|
||||
}
|
||||
}
|
||||
|
||||
void NavObstacle::set_vertices(const Vector<Vector3> &p_vertices) {
|
||||
if (vertices == p_vertices) {
|
||||
return;
|
||||
}
|
||||
|
||||
vertices = p_vertices;
|
||||
obstacle_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
bool NavObstacle::is_map_changed() {
|
||||
if (map) {
|
||||
bool is_changed = map->get_iteration_id() != last_map_iteration_id;
|
||||
last_map_iteration_id = map->get_iteration_id();
|
||||
return is_changed;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void NavObstacle::set_avoidance_layers(uint32_t p_layers) {
|
||||
if (avoidance_layers == p_layers) {
|
||||
return;
|
||||
}
|
||||
|
||||
avoidance_layers = p_layers;
|
||||
obstacle_dirty = true;
|
||||
|
||||
if (agent) {
|
||||
agent->set_avoidance_layers(avoidance_layers);
|
||||
}
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
bool NavObstacle::is_dirty() const {
|
||||
return obstacle_dirty;
|
||||
}
|
||||
|
||||
void NavObstacle::sync() {
|
||||
obstacle_dirty = false;
|
||||
}
|
||||
|
||||
void NavObstacle::internal_update_agent() {
|
||||
if (agent) {
|
||||
agent->set_neighbor_distance(0.0);
|
||||
agent->set_max_neighbors(0.0);
|
||||
agent->set_time_horizon_agents(0.0);
|
||||
agent->set_time_horizon_obstacles(0.0);
|
||||
agent->set_avoidance_mask(0.0);
|
||||
agent->set_neighbor_distance(0.0);
|
||||
agent->set_avoidance_priority(1.0);
|
||||
agent->set_map(map);
|
||||
agent->set_paused(paused);
|
||||
agent->set_radius(radius);
|
||||
agent->set_height(height);
|
||||
agent->set_position(position);
|
||||
agent->set_avoidance_layers(avoidance_layers);
|
||||
agent->set_avoidance_enabled(avoidance_enabled);
|
||||
agent->set_use_3d_avoidance(use_3d_avoidance);
|
||||
}
|
||||
}
|
||||
|
||||
void NavObstacle::set_paused(bool p_paused) {
|
||||
if (paused == p_paused) {
|
||||
return;
|
||||
}
|
||||
|
||||
paused = p_paused;
|
||||
|
||||
if (map) {
|
||||
if (paused) {
|
||||
map->remove_obstacle(this);
|
||||
} else {
|
||||
map->add_obstacle(this);
|
||||
}
|
||||
}
|
||||
internal_update_agent();
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
111
engine/modules/navigation/nav_obstacle.h
Normal file
111
engine/modules/navigation/nav_obstacle.h
Normal file
|
|
@ -0,0 +1,111 @@
|
|||
/**************************************************************************/
|
||||
/* nav_obstacle.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_OBSTACLE_H
|
||||
#define NAV_OBSTACLE_H
|
||||
|
||||
#include "nav_rid.h"
|
||||
|
||||
#include "core/object/class_db.h"
|
||||
#include "core/templates/self_list.h"
|
||||
|
||||
class NavAgent;
|
||||
class NavMap;
|
||||
|
||||
class NavObstacle : public NavRid {
|
||||
NavAgent *agent = nullptr;
|
||||
NavMap *map = nullptr;
|
||||
Vector3 velocity;
|
||||
Vector3 position;
|
||||
Vector<Vector3> vertices;
|
||||
|
||||
real_t radius = 0.0;
|
||||
real_t height = 0.0;
|
||||
|
||||
bool avoidance_enabled = false;
|
||||
bool use_3d_avoidance = false;
|
||||
uint32_t avoidance_layers = 1;
|
||||
|
||||
bool obstacle_dirty = true;
|
||||
|
||||
uint32_t last_map_iteration_id = 0;
|
||||
bool paused = false;
|
||||
|
||||
SelfList<NavObstacle> sync_dirty_request_list_element;
|
||||
|
||||
public:
|
||||
NavObstacle();
|
||||
~NavObstacle();
|
||||
|
||||
void set_avoidance_enabled(bool p_enabled);
|
||||
bool is_avoidance_enabled() { return avoidance_enabled; }
|
||||
|
||||
void set_use_3d_avoidance(bool p_enabled);
|
||||
bool get_use_3d_avoidance() { return use_3d_avoidance; }
|
||||
|
||||
void set_map(NavMap *p_map);
|
||||
NavMap *get_map() { return map; }
|
||||
|
||||
void set_agent(NavAgent *p_agent);
|
||||
NavAgent *get_agent() { return agent; }
|
||||
|
||||
void set_position(const Vector3 p_position);
|
||||
const Vector3 &get_position() const { return position; }
|
||||
|
||||
void set_radius(real_t p_radius);
|
||||
real_t get_radius() const { return radius; }
|
||||
|
||||
void set_height(const real_t p_height);
|
||||
real_t get_height() const { return height; }
|
||||
|
||||
void set_velocity(const Vector3 p_velocity);
|
||||
const Vector3 &get_velocity() const { return velocity; }
|
||||
|
||||
void set_vertices(const Vector<Vector3> &p_vertices);
|
||||
const Vector<Vector3> &get_vertices() const { return vertices; }
|
||||
|
||||
bool is_map_changed();
|
||||
|
||||
void set_avoidance_layers(uint32_t p_layers);
|
||||
uint32_t get_avoidance_layers() const { return avoidance_layers; }
|
||||
|
||||
void set_paused(bool p_paused);
|
||||
bool get_paused() const;
|
||||
|
||||
bool is_dirty() const;
|
||||
void sync();
|
||||
void request_sync();
|
||||
void cancel_sync_request();
|
||||
|
||||
private:
|
||||
void internal_update_agent();
|
||||
};
|
||||
|
||||
#endif // NAV_OBSTACLE_H
|
||||
333
engine/modules/navigation/nav_region.cpp
Normal file
333
engine/modules/navigation/nav_region.cpp
Normal file
|
|
@ -0,0 +1,333 @@
|
|||
/**************************************************************************/
|
||||
/* nav_region.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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#include "nav_region.h"
|
||||
|
||||
#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);
|
||||
}
|
||||
|
||||
map = p_map;
|
||||
polygons_dirty = true;
|
||||
|
||||
if (map) {
|
||||
map->add_region(this);
|
||||
request_sync();
|
||||
}
|
||||
}
|
||||
|
||||
void NavRegion::set_enabled(bool p_enabled) {
|
||||
if (enabled == p_enabled) {
|
||||
return;
|
||||
}
|
||||
enabled = 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) {
|
||||
if (transform == p_transform) {
|
||||
return;
|
||||
}
|
||||
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.");
|
||||
}
|
||||
#endif // DEBUG_ENABLED
|
||||
}
|
||||
|
||||
void NavRegion::set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) {
|
||||
#ifdef DEBUG_ENABLED
|
||||
if (map && p_navigation_mesh.is_valid() && !Math::is_equal_approx(double(map->get_cell_size()), double(p_navigation_mesh->get_cell_size()))) {
|
||||
ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_size` of %s while assigned to a navigation map set to a `cell_size` of %s. The cell size for navigation maps can be changed by using the NavigationServer map_set_cell_size() function. The cell size for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_mesh->get_cell_size()), double(map->get_cell_size())));
|
||||
}
|
||||
|
||||
if (map && p_navigation_mesh.is_valid() && !Math::is_equal_approx(double(map->get_cell_height()), double(p_navigation_mesh->get_cell_height()))) {
|
||||
ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_height` of %s while assigned to a navigation map set to a `cell_height` of %s. The cell height for navigation maps can be changed by using the NavigationServer map_set_cell_height() function. The cell height for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_mesh->get_cell_height()), double(map->get_cell_height())));
|
||||
}
|
||||
#endif // DEBUG_ENABLED
|
||||
|
||||
RWLockWrite write_lock(navmesh_rwlock);
|
||||
|
||||
pending_navmesh_vertices.clear();
|
||||
pending_navmesh_polygons.clear();
|
||||
|
||||
if (p_navigation_mesh.is_valid()) {
|
||||
p_navigation_mesh->get_data(pending_navmesh_vertices, pending_navmesh_polygons);
|
||||
}
|
||||
|
||||
polygons_dirty = true;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
gd::ClosestPointQueryResult NavRegion::get_closest_point_info(const Vector3 &p_point) const {
|
||||
RWLockRead read_lock(region_rwlock);
|
||||
|
||||
return NavMeshQueries3D::polygons_get_closest_point_info(get_polygons(), p_point);
|
||||
}
|
||||
|
||||
Vector3 NavRegion::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
|
||||
RWLockRead read_lock(region_rwlock);
|
||||
|
||||
if (!get_enabled()) {
|
||||
return Vector3();
|
||||
}
|
||||
|
||||
return NavMeshQueries3D::polygons_get_random_point(get_polygons(), p_navigation_layers, p_uniformly);
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
request_sync();
|
||||
}
|
||||
|
||||
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() {
|
||||
RWLockWrite write_lock(region_rwlock);
|
||||
|
||||
bool something_changed = region_dirty || polygons_dirty;
|
||||
|
||||
region_dirty = false;
|
||||
|
||||
update_polygons();
|
||||
|
||||
return something_changed;
|
||||
}
|
||||
|
||||
void NavRegion::update_polygons() {
|
||||
if (!polygons_dirty) {
|
||||
return;
|
||||
}
|
||||
navmesh_polygons.clear();
|
||||
surface_area = 0.0;
|
||||
bounds = AABB();
|
||||
polygons_dirty = false;
|
||||
|
||||
if (map == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
RWLockRead read_lock(navmesh_rwlock);
|
||||
|
||||
if (pending_navmesh_vertices.is_empty() || pending_navmesh_polygons.is_empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
int len = pending_navmesh_vertices.size();
|
||||
if (len == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
const Vector3 *vertices_r = pending_navmesh_vertices.ptr();
|
||||
|
||||
navmesh_polygons.resize(pending_navmesh_polygons.size());
|
||||
|
||||
real_t _new_region_surface_area = 0.0;
|
||||
AABB _new_bounds;
|
||||
|
||||
bool first_vertex = true;
|
||||
int navigation_mesh_polygon_index = 0;
|
||||
|
||||
for (gd::Polygon &polygon : navmesh_polygons) {
|
||||
polygon.surface_area = 0.0;
|
||||
|
||||
Vector<int> navigation_mesh_polygon = pending_navmesh_polygons[navigation_mesh_polygon_index];
|
||||
navigation_mesh_polygon_index += 1;
|
||||
|
||||
int navigation_mesh_polygon_size = navigation_mesh_polygon.size();
|
||||
if (navigation_mesh_polygon_size < 3) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const int *indices = navigation_mesh_polygon.ptr();
|
||||
bool valid(true);
|
||||
|
||||
polygon.points.resize(navigation_mesh_polygon_size);
|
||||
polygon.edges.resize(navigation_mesh_polygon_size);
|
||||
|
||||
real_t _new_polygon_surface_area = 0.0;
|
||||
|
||||
for (int j(2); j < navigation_mesh_polygon_size; j++) {
|
||||
const Face3 face = Face3(
|
||||
transform.xform(vertices_r[indices[0]]),
|
||||
transform.xform(vertices_r[indices[j - 1]]),
|
||||
transform.xform(vertices_r[indices[j]]));
|
||||
|
||||
_new_polygon_surface_area += face.get_area();
|
||||
}
|
||||
|
||||
polygon.surface_area = _new_polygon_surface_area;
|
||||
_new_region_surface_area += _new_polygon_surface_area;
|
||||
|
||||
for (int j(0); j < navigation_mesh_polygon_size; j++) {
|
||||
int idx = indices[j];
|
||||
if (idx < 0 || idx >= len) {
|
||||
valid = false;
|
||||
break;
|
||||
}
|
||||
|
||||
Vector3 point_position = transform.xform(vertices_r[idx]);
|
||||
polygon.points[j].pos = 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) {
|
||||
ERR_BREAK_MSG(!valid, "The navigation mesh set in this region is not valid!");
|
||||
}
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
118
engine/modules/navigation/nav_region.h
Normal file
118
engine/modules/navigation/nav_region.h
Normal file
|
|
@ -0,0 +1,118 @@
|
|||
/**************************************************************************/
|
||||
/* nav_region.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_H
|
||||
#define NAV_REGION_H
|
||||
|
||||
#include "nav_base.h"
|
||||
#include "nav_utils.h"
|
||||
|
||||
#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;
|
||||
bool enabled = true;
|
||||
|
||||
bool use_edge_connections = true;
|
||||
|
||||
bool region_dirty = true;
|
||||
bool polygons_dirty = true;
|
||||
|
||||
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();
|
||||
~NavRegion();
|
||||
|
||||
void scratch_polygons() {
|
||||
polygons_dirty = true;
|
||||
}
|
||||
|
||||
void set_enabled(bool p_enabled);
|
||||
bool get_enabled() const { return enabled; }
|
||||
|
||||
void set_map(NavMap *p_map);
|
||||
NavMap *get_map() const {
|
||||
return map;
|
||||
}
|
||||
|
||||
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 {
|
||||
return transform;
|
||||
}
|
||||
|
||||
void set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh);
|
||||
|
||||
LocalVector<gd::Polygon> const &get_polygons() const {
|
||||
return navmesh_polygons;
|
||||
}
|
||||
|
||||
Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const;
|
||||
gd::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
|
||||
Vector3 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const;
|
||||
|
||||
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();
|
||||
};
|
||||
|
||||
#endif // NAV_REGION_H
|
||||
44
engine/modules/navigation/nav_rid.h
Normal file
44
engine/modules/navigation/nav_rid.h
Normal file
|
|
@ -0,0 +1,44 @@
|
|||
/**************************************************************************/
|
||||
/* nav_rid.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_RID_H
|
||||
#define NAV_RID_H
|
||||
|
||||
#include "core/templates/rid.h"
|
||||
|
||||
class NavRid {
|
||||
RID self;
|
||||
|
||||
public:
|
||||
_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
|
||||
_FORCE_INLINE_ RID get_self() const { return self; }
|
||||
};
|
||||
|
||||
#endif // NAV_RID_H
|
||||
344
engine/modules/navigation/nav_utils.h
Normal file
344
engine/modules/navigation/nav_utils.h
Normal file
|
|
@ -0,0 +1,344 @@
|
|||
/**************************************************************************/
|
||||
/* nav_utils.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_UTILS_H
|
||||
#define NAV_UTILS_H
|
||||
|
||||
#include "core/math/vector3.h"
|
||||
#include "core/templates/hash_map.h"
|
||||
#include "core/templates/hashfuncs.h"
|
||||
#include "core/templates/local_vector.h"
|
||||
#include "servers/navigation/navigation_utilities.h"
|
||||
|
||||
struct NavBaseIteration;
|
||||
|
||||
namespace gd {
|
||||
struct Polygon;
|
||||
|
||||
union PointKey {
|
||||
struct {
|
||||
int64_t x : 21;
|
||||
int64_t y : 22;
|
||||
int64_t z : 21;
|
||||
};
|
||||
|
||||
uint64_t key = 0;
|
||||
};
|
||||
|
||||
struct EdgeKey {
|
||||
PointKey a;
|
||||
PointKey b;
|
||||
|
||||
static uint32_t hash(const EdgeKey &p_val) {
|
||||
return hash_one_uint64(p_val.a.key) ^ hash_one_uint64(p_val.b.key);
|
||||
}
|
||||
|
||||
bool operator==(const EdgeKey &p_key) const {
|
||||
return (a.key == p_key.a.key) && (b.key == p_key.b.key);
|
||||
}
|
||||
|
||||
EdgeKey(const PointKey &p_a = PointKey(), const PointKey &p_b = PointKey()) :
|
||||
a(p_a),
|
||||
b(p_b) {
|
||||
if (a.key > b.key) {
|
||||
SWAP(a, b);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
struct Point {
|
||||
Vector3 pos;
|
||||
PointKey key;
|
||||
};
|
||||
|
||||
struct Edge {
|
||||
/// The gateway in the edge, as, in some case, the whole edge might not be navigable.
|
||||
struct Connection {
|
||||
/// Polygon that this connection leads to.
|
||||
Polygon *polygon = nullptr;
|
||||
|
||||
/// Edge of the source polygon where this connection starts from.
|
||||
int edge = -1;
|
||||
|
||||
/// Point on the edge where the gateway leading to the poly starts.
|
||||
Vector3 pathway_start;
|
||||
|
||||
/// Point on the edge where the gateway leading to the poly ends.
|
||||
Vector3 pathway_end;
|
||||
};
|
||||
|
||||
/// Connections from this edge to other polygons.
|
||||
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 NavBaseIteration *owner = nullptr;
|
||||
|
||||
/// The points of this `Polygon`
|
||||
LocalVector<Point> points;
|
||||
|
||||
/// The edges of this `Polygon`
|
||||
LocalVector<Edge> edges;
|
||||
|
||||
real_t surface_area = 0.0;
|
||||
};
|
||||
|
||||
struct NavigationPoly {
|
||||
/// This 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;
|
||||
int back_navigation_edge = -1;
|
||||
Vector3 back_navigation_edge_pathway_start;
|
||||
Vector3 back_navigation_edge_pathway_end;
|
||||
|
||||
/// The entry position of this poly.
|
||||
Vector3 entry;
|
||||
/// 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;
|
||||
|
||||
/// The total travel cost (f cost).
|
||||
real_t total_travel_cost() const {
|
||||
return traveled_distance + distance_to_destination;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
};
|
||||
|
||||
struct ClosestPointQueryResult {
|
||||
Vector3 point;
|
||||
Vector3 normal;
|
||||
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
|
||||
97
engine/modules/navigation/register_types.cpp
Normal file
97
engine/modules/navigation/register_types.cpp
Normal file
|
|
@ -0,0 +1,97 @@
|
|||
/**************************************************************************/
|
||||
/* register_types.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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#include "register_types.h"
|
||||
|
||||
#include "2d/godot_navigation_server_2d.h"
|
||||
#include "3d/godot_navigation_server_3d.h"
|
||||
|
||||
#ifndef DISABLE_DEPRECATED
|
||||
#ifndef _3D_DISABLED
|
||||
#include "3d/navigation_mesh_generator.h"
|
||||
#endif
|
||||
#endif // DISABLE_DEPRECATED
|
||||
|
||||
#ifdef TOOLS_ENABLED
|
||||
#include "editor/navigation_mesh_editor_plugin.h"
|
||||
#endif
|
||||
|
||||
#include "core/config/engine.h"
|
||||
#include "servers/navigation_server_2d.h"
|
||||
#include "servers/navigation_server_3d.h"
|
||||
|
||||
#ifndef DISABLE_DEPRECATED
|
||||
#ifndef _3D_DISABLED
|
||||
NavigationMeshGenerator *_nav_mesh_generator = nullptr;
|
||||
#endif
|
||||
#endif // DISABLE_DEPRECATED
|
||||
|
||||
NavigationServer3D *new_navigation_server_3d() {
|
||||
return memnew(GodotNavigationServer3D);
|
||||
}
|
||||
|
||||
NavigationServer2D *new_navigation_server_2d() {
|
||||
return memnew(GodotNavigationServer2D);
|
||||
}
|
||||
|
||||
void initialize_navigation_module(ModuleInitializationLevel p_level) {
|
||||
if (p_level == MODULE_INITIALIZATION_LEVEL_SERVERS) {
|
||||
NavigationServer3DManager::set_default_server(new_navigation_server_3d);
|
||||
NavigationServer2DManager::set_default_server(new_navigation_server_2d);
|
||||
|
||||
#ifndef DISABLE_DEPRECATED
|
||||
#ifndef _3D_DISABLED
|
||||
_nav_mesh_generator = memnew(NavigationMeshGenerator);
|
||||
GDREGISTER_CLASS(NavigationMeshGenerator);
|
||||
Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationMeshGenerator", NavigationMeshGenerator::get_singleton()));
|
||||
#endif
|
||||
#endif // DISABLE_DEPRECATED
|
||||
}
|
||||
|
||||
#ifdef TOOLS_ENABLED
|
||||
if (p_level == MODULE_INITIALIZATION_LEVEL_EDITOR) {
|
||||
EditorPlugins::add_by_type<NavigationMeshEditorPlugin>();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void uninitialize_navigation_module(ModuleInitializationLevel p_level) {
|
||||
if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) {
|
||||
return;
|
||||
}
|
||||
|
||||
#ifndef DISABLE_DEPRECATED
|
||||
#ifndef _3D_DISABLED
|
||||
if (_nav_mesh_generator) {
|
||||
memdelete(_nav_mesh_generator);
|
||||
}
|
||||
#endif
|
||||
#endif // DISABLE_DEPRECATED
|
||||
}
|
||||
39
engine/modules/navigation/register_types.h
Normal file
39
engine/modules/navigation/register_types.h
Normal file
|
|
@ -0,0 +1,39 @@
|
|||
/**************************************************************************/
|
||||
/* register_types.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 NAVIGATION_REGISTER_TYPES_H
|
||||
#define NAVIGATION_REGISTER_TYPES_H
|
||||
|
||||
#include "modules/register_module_types.h"
|
||||
|
||||
void initialize_navigation_module(ModuleInitializationLevel p_level);
|
||||
void uninitialize_navigation_module(ModuleInitializationLevel p_level);
|
||||
|
||||
#endif // NAVIGATION_REGISTER_TYPES_H
|
||||
Loading…
Add table
Add a link
Reference in a new issue