summaryrefslogtreecommitdiff
path: root/modules/navigation
diff options
context:
space:
mode:
Diffstat (limited to 'modules/navigation')
-rw-r--r--modules/navigation/SCsub63
-rw-r--r--modules/navigation/config.py6
-rw-r--r--modules/navigation/godot_navigation_server.cpp547
-rw-r--r--modules/navigation/godot_navigation_server.h149
-rw-r--r--modules/navigation/nav_map.cpp764
-rw-r--r--modules/navigation/nav_map.h141
-rw-r--r--modules/navigation/nav_region.cpp161
-rw-r--r--modules/navigation/nav_region.h101
-rw-r--r--modules/navigation/nav_rid.h48
-rw-r--r--modules/navigation/nav_utils.h140
-rw-r--r--modules/navigation/navigation_mesh_editor_plugin.cpp154
-rw-r--r--modules/navigation/navigation_mesh_editor_plugin.h88
-rw-r--r--modules/navigation/navigation_mesh_generator.cpp583
-rw-r--r--modules/navigation/navigation_mesh_generator.h83
-rw-r--r--modules/navigation/register_types.cpp74
-rw-r--r--modules/navigation/register_types.h37
-rw-r--r--modules/navigation/rvo_agent.cpp83
-rw-r--r--modules/navigation/rvo_agent.h78
18 files changed, 3300 insertions, 0 deletions
diff --git a/modules/navigation/SCsub b/modules/navigation/SCsub
new file mode 100644
index 0000000000..22b5509b32
--- /dev/null
+++ b/modules/navigation/SCsub
@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+
+Import("env")
+Import("env_modules")
+
+env_navigation = env_modules.Clone()
+
+# Thirdparty source files
+
+thirdparty_obj = []
+
+# Recast Thirdparty source files
+if env["builtin_recast"]:
+ 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 Thirdparty source files
+if env["builtin_rvo2"]:
+ thirdparty_dir = "#thirdparty/rvo2/"
+ thirdparty_sources = [
+ "Agent.cpp",
+ "KdTree.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.modules_sources += module_obj
+
+# Needed to force rebuilding the module files when the thirdparty library is updated.
+env.Depends(module_obj, thirdparty_obj)
diff --git a/modules/navigation/config.py b/modules/navigation/config.py
new file mode 100644
index 0000000000..d22f9454ed
--- /dev/null
+++ b/modules/navigation/config.py
@@ -0,0 +1,6 @@
+def can_build(env, platform):
+ return True
+
+
+def configure(env):
+ pass
diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
new file mode 100644
index 0000000000..df003cfe6f
--- /dev/null
+++ b/modules/navigation/godot_navigation_server.cpp
@@ -0,0 +1,547 @@
+/*************************************************************************/
+/* godot_navigation_server.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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.h"
+
+#include "core/os/mutex.h"
+
+#ifndef _3D_DISABLED
+#include "navigation_mesh_generator.h"
+#endif
+
+/**
+ @author AndreaCatania
+*/
+
+/// Creates a struct for each function and a function that once called creates
+/// an instance of that struct with the submitted parameters.
+/// Then, that struct is stored in an array; the `sync` function consume that array.
+
+#define COMMAND_1(F_NAME, T_0, D_0) \
+ struct MERGE(F_NAME, _command) : public SetCommand { \
+ T_0 d_0; \
+ MERGE(F_NAME, _command) \
+ (T_0 p_d_0) : \
+ d_0(p_d_0) {} \
+ virtual void exec(GodotNavigationServer *server) { \
+ server->MERGE(_cmd_, F_NAME)(d_0); \
+ } \
+ }; \
+ void GodotNavigationServer::F_NAME(T_0 D_0) const { \
+ auto cmd = memnew(MERGE(F_NAME, _command)( \
+ D_0)); \
+ add_command(cmd); \
+ } \
+ void GodotNavigationServer::MERGE(_cmd_, F_NAME)(T_0 D_0)
+
+#define COMMAND_2(F_NAME, T_0, D_0, T_1, D_1) \
+ struct MERGE(F_NAME, _command) : public SetCommand { \
+ T_0 d_0; \
+ T_1 d_1; \
+ MERGE(F_NAME, _command) \
+ ( \
+ T_0 p_d_0, \
+ T_1 p_d_1) : \
+ d_0(p_d_0), \
+ d_1(p_d_1) {} \
+ virtual void exec(GodotNavigationServer *server) { \
+ server->MERGE(_cmd_, F_NAME)(d_0, d_1); \
+ } \
+ }; \
+ void GodotNavigationServer::F_NAME(T_0 D_0, T_1 D_1) const { \
+ auto cmd = memnew(MERGE(F_NAME, _command)( \
+ D_0, \
+ D_1)); \
+ add_command(cmd); \
+ } \
+ void GodotNavigationServer::MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1)
+
+#define COMMAND_4(F_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3) \
+ struct MERGE(F_NAME, _command) : public SetCommand { \
+ T_0 d_0; \
+ T_1 d_1; \
+ T_2 d_2; \
+ T_3 d_3; \
+ MERGE(F_NAME, _command) \
+ ( \
+ T_0 p_d_0, \
+ T_1 p_d_1, \
+ T_2 p_d_2, \
+ T_3 p_d_3) : \
+ d_0(p_d_0), \
+ d_1(p_d_1), \
+ d_2(p_d_2), \
+ d_3(p_d_3) {} \
+ virtual void exec(GodotNavigationServer *server) { \
+ server->MERGE(_cmd_, F_NAME)(d_0, d_1, d_2, d_3); \
+ } \
+ }; \
+ void GodotNavigationServer::F_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3) const { \
+ auto cmd = memnew(MERGE(F_NAME, _command)( \
+ D_0, \
+ D_1, \
+ D_2, \
+ D_3)); \
+ add_command(cmd); \
+ } \
+ void GodotNavigationServer::MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3)
+
+GodotNavigationServer::GodotNavigationServer() :
+ NavigationServer3D() {
+}
+
+GodotNavigationServer::~GodotNavigationServer() {
+ flush_queries();
+}
+
+void GodotNavigationServer::add_command(SetCommand *command) const {
+ GodotNavigationServer *mut_this = const_cast<GodotNavigationServer *>(this);
+ {
+ MutexLock lock(commands_mutex);
+ mut_this->commands.push_back(command);
+ }
+}
+
+RID GodotNavigationServer::map_create() const {
+ GodotNavigationServer *mut_this = const_cast<GodotNavigationServer *>(this);
+ MutexLock lock(mut_this->operations_mutex);
+ RID rid = map_owner.make_rid();
+ NavMap *space = map_owner.getornull(rid);
+ space->set_self(rid);
+ return rid;
+}
+
+COMMAND_2(map_set_active, RID, p_map, bool, p_active) {
+ NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND(map == nullptr);
+
+ if (p_active) {
+ if (!map_is_active(p_map)) {
+ active_maps.push_back(map);
+ active_maps_update_id.push_back(map->get_map_update_id());
+ }
+ } else {
+ int map_index = active_maps.find(map);
+ ERR_FAIL_COND(map_index < 0);
+ active_maps.remove(map_index);
+ active_maps_update_id.remove(map_index);
+ }
+}
+
+bool GodotNavigationServer::map_is_active(RID p_map) const {
+ NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND_V(map == nullptr, false);
+
+ return active_maps.find(map) >= 0;
+}
+
+COMMAND_2(map_set_up, RID, p_map, Vector3, p_up) {
+ NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND(map == nullptr);
+
+ map->set_up(p_up);
+}
+
+Vector3 GodotNavigationServer::map_get_up(RID p_map) const {
+ const NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND_V(map == nullptr, Vector3());
+
+ return map->get_up();
+}
+
+COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size) {
+ NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND(map == nullptr);
+
+ map->set_cell_size(p_cell_size);
+}
+
+real_t GodotNavigationServer::map_get_cell_size(RID p_map) const {
+ const NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND_V(map == nullptr, 0);
+
+ return map->get_cell_size();
+}
+
+COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin) {
+ NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND(map == nullptr);
+
+ map->set_edge_connection_margin(p_connection_margin);
+}
+
+real_t GodotNavigationServer::map_get_edge_connection_margin(RID p_map) const {
+ const NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND_V(map == nullptr, 0);
+
+ return map->get_edge_connection_margin();
+}
+
+Vector<Vector3> GodotNavigationServer::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers) const {
+ const NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND_V(map == nullptr, Vector<Vector3>());
+
+ return map->get_path(p_origin, p_destination, p_optimize, p_layers);
+}
+
+Vector3 GodotNavigationServer::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
+ const NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND_V(map == nullptr, Vector3());
+
+ return map->get_closest_point_to_segment(p_from, p_to, p_use_collision);
+}
+
+Vector3 GodotNavigationServer::map_get_closest_point(RID p_map, const Vector3 &p_point) const {
+ const NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND_V(map == nullptr, Vector3());
+
+ return map->get_closest_point(p_point);
+}
+
+Vector3 GodotNavigationServer::map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const {
+ const NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND_V(map == nullptr, Vector3());
+
+ return map->get_closest_point_normal(p_point);
+}
+
+RID GodotNavigationServer::map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const {
+ const NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND_V(map == nullptr, RID());
+
+ return map->get_closest_point_owner(p_point);
+}
+
+RID GodotNavigationServer::region_create() const {
+ GodotNavigationServer *mut_this = const_cast<GodotNavigationServer *>(this);
+ MutexLock lock(mut_this->operations_mutex);
+ RID rid = region_owner.make_rid();
+ NavRegion *reg = region_owner.getornull(rid);
+ reg->set_self(rid);
+ return rid;
+}
+
+COMMAND_2(region_set_map, RID, p_region, RID, p_map) {
+ NavRegion *region = region_owner.getornull(p_region);
+ ERR_FAIL_COND(region == nullptr);
+
+ if (region->get_map() != nullptr) {
+ if (region->get_map()->get_self() == p_map) {
+ return; // Pointless
+ }
+
+ region->get_map()->remove_region(region);
+ region->set_map(nullptr);
+ }
+
+ if (p_map.is_valid()) {
+ NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND(map == nullptr);
+
+ map->add_region(region);
+ region->set_map(map);
+ }
+}
+
+COMMAND_2(region_set_transform, RID, p_region, Transform3D, p_transform) {
+ NavRegion *region = region_owner.getornull(p_region);
+ ERR_FAIL_COND(region == nullptr);
+
+ region->set_transform(p_transform);
+}
+
+COMMAND_2(region_set_layers, RID, p_region, uint32_t, p_layers) {
+ NavRegion *region = region_owner.getornull(p_region);
+ ERR_FAIL_COND(region == nullptr);
+
+ region->set_layers(p_layers);
+}
+
+uint32_t GodotNavigationServer::region_get_layers(RID p_region) const {
+ NavRegion *region = region_owner.getornull(p_region);
+ ERR_FAIL_COND_V(region == nullptr, 0);
+
+ return region->get_layers();
+}
+
+COMMAND_2(region_set_navmesh, RID, p_region, Ref<NavigationMesh>, p_nav_mesh) {
+ NavRegion *region = region_owner.getornull(p_region);
+ ERR_FAIL_COND(region == nullptr);
+
+ region->set_mesh(p_nav_mesh);
+}
+
+void GodotNavigationServer::region_bake_navmesh(Ref<NavigationMesh> r_mesh, Node *p_node) const {
+ ERR_FAIL_COND(r_mesh.is_null());
+ ERR_FAIL_COND(p_node == nullptr);
+
+#ifndef _3D_DISABLED
+ NavigationMeshGenerator::get_singleton()->clear(r_mesh);
+ NavigationMeshGenerator::get_singleton()->bake(r_mesh, p_node);
+#endif
+}
+
+int GodotNavigationServer::region_get_connections_count(RID p_region) const {
+ NavRegion *region = region_owner.getornull(p_region);
+ ERR_FAIL_COND_V(!region, 0);
+
+ return region->get_connections_count();
+}
+
+Vector3 GodotNavigationServer::region_get_connection_pathway_start(RID p_region, int p_connection_id) const {
+ NavRegion *region = region_owner.getornull(p_region);
+ ERR_FAIL_COND_V(!region, Vector3());
+
+ return region->get_connection_pathway_start(p_connection_id);
+}
+
+Vector3 GodotNavigationServer::region_get_connection_pathway_end(RID p_region, int p_connection_id) const {
+ NavRegion *region = region_owner.getornull(p_region);
+ ERR_FAIL_COND_V(!region, Vector3());
+
+ return region->get_connection_pathway_end(p_connection_id);
+}
+
+RID GodotNavigationServer::agent_create() const {
+ GodotNavigationServer *mut_this = const_cast<GodotNavigationServer *>(this);
+ MutexLock lock(mut_this->operations_mutex);
+ RID rid = agent_owner.make_rid();
+ RvoAgent *agent = agent_owner.getornull(rid);
+ agent->set_self(rid);
+ return rid;
+}
+
+COMMAND_2(agent_set_map, RID, p_agent, RID, p_map) {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND(agent == nullptr);
+
+ if (agent->get_map()) {
+ if (agent->get_map()->get_self() == p_map) {
+ return; // Pointless
+ }
+
+ agent->get_map()->remove_agent(agent);
+ }
+
+ agent->set_map(nullptr);
+
+ if (p_map.is_valid()) {
+ NavMap *map = map_owner.getornull(p_map);
+ ERR_FAIL_COND(map == nullptr);
+
+ agent->set_map(map);
+ map->add_agent(agent);
+
+ if (agent->has_callback()) {
+ map->set_agent_as_controlled(agent);
+ }
+ }
+}
+
+COMMAND_2(agent_set_neighbor_dist, RID, p_agent, real_t, p_dist) {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND(agent == nullptr);
+
+ agent->get_agent()->neighborDist_ = p_dist;
+}
+
+COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count) {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND(agent == nullptr);
+
+ agent->get_agent()->maxNeighbors_ = p_count;
+}
+
+COMMAND_2(agent_set_time_horizon, RID, p_agent, real_t, p_time) {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND(agent == nullptr);
+
+ agent->get_agent()->timeHorizon_ = p_time;
+}
+
+COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius) {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND(agent == nullptr);
+
+ agent->get_agent()->radius_ = p_radius;
+}
+
+COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed) {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND(agent == nullptr);
+
+ agent->get_agent()->maxSpeed_ = p_max_speed;
+}
+
+COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity) {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND(agent == nullptr);
+
+ agent->get_agent()->velocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
+}
+
+COMMAND_2(agent_set_target_velocity, RID, p_agent, Vector3, p_velocity) {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND(agent == nullptr);
+
+ agent->get_agent()->prefVelocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
+}
+
+COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position) {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND(agent == nullptr);
+
+ agent->get_agent()->position_ = RVO::Vector3(p_position.x, p_position.y, p_position.z);
+}
+
+COMMAND_2(agent_set_ignore_y, RID, p_agent, bool, p_ignore) {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND(agent == nullptr);
+
+ agent->get_agent()->ignore_y_ = p_ignore;
+}
+
+bool GodotNavigationServer::agent_is_map_changed(RID p_agent) const {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND_V(agent == nullptr, false);
+
+ return agent->is_map_changed();
+}
+
+COMMAND_4(agent_set_callback, RID, p_agent, Object *, p_receiver, StringName, p_method, Variant, p_udata) {
+ RvoAgent *agent = agent_owner.getornull(p_agent);
+ ERR_FAIL_COND(agent == nullptr);
+
+ agent->set_callback(p_receiver == nullptr ? ObjectID() : p_receiver->get_instance_id(), p_method, p_udata);
+
+ if (agent->get_map()) {
+ if (p_receiver == nullptr) {
+ agent->get_map()->remove_agent_as_controlled(agent);
+ } else {
+ agent->get_map()->set_agent_as_controlled(agent);
+ }
+ }
+}
+
+COMMAND_1(free, RID, p_object) {
+ if (map_owner.owns(p_object)) {
+ NavMap *map = map_owner.getornull(p_object);
+
+ // Removes any assigned region
+ std::vector<NavRegion *> regions = map->get_regions();
+ for (size_t i(0); i < regions.size(); i++) {
+ map->remove_region(regions[i]);
+ regions[i]->set_map(nullptr);
+ }
+
+ // Remove any assigned agent
+ std::vector<RvoAgent *> agents = map->get_agents();
+ for (size_t i(0); i < agents.size(); i++) {
+ map->remove_agent(agents[i]);
+ agents[i]->set_map(nullptr);
+ }
+
+ int map_index = active_maps.find(map);
+ active_maps.remove(map_index);
+ active_maps_update_id.remove(map_index);
+ map_owner.free(p_object);
+
+ } else if (region_owner.owns(p_object)) {
+ NavRegion *region = region_owner.getornull(p_object);
+
+ // Removes this region from the map if assigned
+ if (region->get_map() != nullptr) {
+ region->get_map()->remove_region(region);
+ region->set_map(nullptr);
+ }
+
+ region_owner.free(p_object);
+
+ } else if (agent_owner.owns(p_object)) {
+ RvoAgent *agent = agent_owner.getornull(p_object);
+
+ // Removes this agent from the map if assigned
+ if (agent->get_map() != nullptr) {
+ agent->get_map()->remove_agent(agent);
+ agent->set_map(nullptr);
+ }
+
+ agent_owner.free(p_object);
+
+ } else {
+ ERR_FAIL_COND("Invalid ID.");
+ }
+}
+
+void GodotNavigationServer::set_active(bool p_active) const {
+ GodotNavigationServer *mut_this = const_cast<GodotNavigationServer *>(this);
+ MutexLock lock(mut_this->operations_mutex);
+ mut_this->active = p_active;
+}
+
+void GodotNavigationServer::flush_queries() {
+ // In c++ we can't be sure that this is performed in the main thread
+ // even with mutable functions.
+ MutexLock lock(commands_mutex);
+ MutexLock lock2(operations_mutex);
+ for (size_t i(0); i < commands.size(); i++) {
+ commands[i]->exec(this);
+ memdelete(commands[i]);
+ }
+ commands.clear();
+}
+
+void GodotNavigationServer::process(real_t p_delta_time) {
+ flush_queries();
+
+ if (!active) {
+ return;
+ }
+
+ // In c++ we can't be sure that this is performed in the main thread
+ // even with mutable functions.
+ MutexLock lock(operations_mutex);
+ for (uint32_t i(0); i < active_maps.size(); i++) {
+ active_maps[i]->sync();
+ active_maps[i]->step(p_delta_time);
+ active_maps[i]->dispatch_callbacks();
+
+ // Emit a signal if a map changed.
+ const uint32_t new_map_update_id = active_maps[i]->get_map_update_id();
+ if (new_map_update_id != active_maps_update_id[i]) {
+ emit_signal("map_changed", active_maps[i]->get_self());
+ active_maps_update_id[i] = new_map_update_id;
+ }
+ }
+}
+
+#undef COMMAND_1
+#undef COMMAND_2
+#undef COMMAND_4
diff --git a/modules/navigation/godot_navigation_server.h b/modules/navigation/godot_navigation_server.h
new file mode 100644
index 0000000000..65224493fd
--- /dev/null
+++ b/modules/navigation/godot_navigation_server.h
@@ -0,0 +1,149 @@
+/*************************************************************************/
+/* godot_navigation_server.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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_H
+#define GODOT_NAVIGATION_SERVER_H
+
+#include "core/templates/local_vector.h"
+#include "core/templates/rid.h"
+#include "core/templates/rid_owner.h"
+#include "servers/navigation_server_3d.h"
+
+#include "nav_map.h"
+#include "nav_region.h"
+#include "rvo_agent.h"
+
+/**
+ @author AndreaCatania
+*/
+
+/// 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) const; \
+ 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) const; \
+ void MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1)
+
+#define COMMAND_4_DEF(F_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3, D_3_DEF) \
+ virtual void F_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3 = D_3_DEF) const; \
+ void MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3)
+
+class GodotNavigationServer;
+
+struct SetCommand {
+ virtual ~SetCommand() {}
+ virtual void exec(GodotNavigationServer *server) = 0;
+};
+
+class GodotNavigationServer : public NavigationServer3D {
+ Mutex commands_mutex;
+ /// Mutex used to make any operation threadsafe.
+ Mutex operations_mutex;
+
+ std::vector<SetCommand *> commands;
+
+ mutable RID_Owner<NavMap> map_owner;
+ mutable RID_Owner<NavRegion> region_owner;
+ mutable RID_Owner<RvoAgent> agent_owner;
+
+ bool active = true;
+ LocalVector<NavMap *> active_maps;
+ LocalVector<uint32_t> active_maps_update_id;
+
+public:
+ GodotNavigationServer();
+ virtual ~GodotNavigationServer();
+
+ void add_command(SetCommand *command) const;
+
+ virtual RID map_create() const;
+ COMMAND_2(map_set_active, RID, p_map, bool, p_active);
+ virtual bool map_is_active(RID p_map) const;
+
+ COMMAND_2(map_set_up, RID, p_map, Vector3, p_up);
+ virtual Vector3 map_get_up(RID p_map) const;
+
+ 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;
+
+ 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;
+
+ virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers = 1) const;
+
+ 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;
+ virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const;
+ virtual Vector3 map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const;
+ virtual RID map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const;
+
+ virtual RID region_create() const;
+ COMMAND_2(region_set_map, RID, p_region, RID, p_map);
+ COMMAND_2(region_set_layers, RID, p_region, uint32_t, p_layers);
+ virtual uint32_t region_get_layers(RID p_region) const;
+ COMMAND_2(region_set_transform, RID, p_region, Transform3D, p_transform);
+ COMMAND_2(region_set_navmesh, RID, p_region, Ref<NavigationMesh>, p_nav_mesh);
+ virtual void region_bake_navmesh(Ref<NavigationMesh> r_mesh, Node *p_node) const;
+ virtual int region_get_connections_count(RID p_region) const;
+ virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const;
+ virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const;
+
+ virtual RID agent_create() const;
+ COMMAND_2(agent_set_map, RID, p_agent, RID, p_map);
+ COMMAND_2(agent_set_neighbor_dist, RID, p_agent, real_t, p_dist);
+ COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count);
+ COMMAND_2(agent_set_time_horizon, RID, p_agent, real_t, p_time);
+ COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius);
+ COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed);
+ COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity);
+ COMMAND_2(agent_set_target_velocity, RID, p_agent, Vector3, p_velocity);
+ COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position);
+ COMMAND_2(agent_set_ignore_y, RID, p_agent, bool, p_ignore);
+ virtual bool agent_is_map_changed(RID p_agent) const;
+ COMMAND_4_DEF(agent_set_callback, RID, p_agent, Object *, p_receiver, StringName, p_method, Variant, p_udata, Variant());
+
+ COMMAND_1(free, RID, p_object);
+
+ virtual void set_active(bool p_active) const;
+
+ void flush_queries();
+ virtual void process(real_t p_delta_time);
+};
+
+#undef COMMAND_1
+#undef COMMAND_2
+#undef COMMAND_4_DEF
+
+#endif // GODOT_NAVIGATION_SERVER_H
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
new file mode 100644
index 0000000000..41306f0687
--- /dev/null
+++ b/modules/navigation/nav_map.cpp
@@ -0,0 +1,764 @@
+/*************************************************************************/
+/* nav_map.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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 "core/os/threaded_array_processor.h"
+#include "nav_region.h"
+#include "rvo_agent.h"
+
+#include <algorithm>
+
+/**
+ @author AndreaCatania
+*/
+
+#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
+
+void NavMap::set_up(Vector3 p_up) {
+ up = p_up;
+ regenerate_polygons = true;
+}
+
+void NavMap::set_cell_size(float p_cell_size) {
+ cell_size = p_cell_size;
+ regenerate_polygons = true;
+}
+
+void NavMap::set_edge_connection_margin(float p_edge_connection_margin) {
+ edge_connection_margin = p_edge_connection_margin;
+ regenerate_links = true;
+}
+
+gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
+ const int x = int(Math::floor(p_pos.x / cell_size));
+ const int y = int(Math::floor(p_pos.y / cell_size));
+ const int z = int(Math::floor(p_pos.z / cell_size));
+
+ gd::PointKey p;
+ p.key = 0;
+ p.x = x;
+ p.y = y;
+ p.z = z;
+ return p;
+}
+
+Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers) const {
+ // Find the start poly and the end poly on this map.
+ const gd::Polygon *begin_poly = nullptr;
+ const gd::Polygon *end_poly = nullptr;
+ Vector3 begin_point;
+ Vector3 end_point;
+ float begin_d = 1e20;
+ float end_d = 1e20;
+ // Find the initial poly and the end poly on this map.
+ for (size_t i(0); i < polygons.size(); i++) {
+ const gd::Polygon &p = polygons[i];
+
+ // Only consider the polygon if it in a region with compatible layers.
+ if ((p_layers & p.owner->get_layers()) == 0) {
+ continue;
+ }
+
+ // For each point cast a face and check the distance between the origin/destination
+ for (size_t point_id = 0; point_id < p.points.size(); point_id++) {
+ const Vector3 p1 = p.points[point_id].pos;
+ const Vector3 p2 = p.points[(point_id + 1) % p.points.size()].pos;
+ const Vector3 p3 = p.points[(point_id + 2) % p.points.size()].pos;
+ const Face3 face(p1, p2, p3);
+
+ Vector3 point = face.get_closest_point_to(p_origin);
+ float distance_to_point = point.distance_to(p_origin);
+ if (distance_to_point < begin_d) {
+ begin_d = distance_to_point;
+ begin_poly = &p;
+ begin_point = point;
+ }
+
+ point = face.get_closest_point_to(p_destination);
+ distance_to_point = point.distance_to(p_destination);
+ if (distance_to_point < end_d) {
+ end_d = distance_to_point;
+ end_poly = &p;
+ end_point = point;
+ }
+ }
+ }
+
+ // Check for trivial cases
+ if (!begin_poly || !end_poly) {
+ return Vector<Vector3>();
+ }
+ if (begin_poly == end_poly) {
+ Vector<Vector3> path;
+ path.resize(2);
+ path.write[0] = begin_point;
+ path.write[1] = end_point;
+ return path;
+ }
+
+ // List of all reachable navigation polys.
+ std::vector<gd::NavigationPoly> navigation_polys;
+ navigation_polys.reserve(polygons.size() * 0.75);
+
+ // Add the start polygon to the reachable navigation polygons.
+ gd::NavigationPoly begin_navigation_poly = gd::NavigationPoly(begin_poly);
+ begin_navigation_poly.self_id = 0;
+ begin_navigation_poly.entry = begin_point;
+ begin_navigation_poly.back_navigation_edge_pathway_start = begin_point;
+ begin_navigation_poly.back_navigation_edge_pathway_end = begin_point;
+ navigation_polys.push_back(begin_navigation_poly);
+
+ // List of polygon IDs to visit.
+ List<uint32_t> to_visit;
+ to_visit.push_back(0);
+
+ // This is an implementation of the A* algorithm.
+ int least_cost_id = 0;
+ bool found_route = false;
+
+ const gd::Polygon *reachable_end = nullptr;
+ float reachable_d = 1e30;
+ bool is_reachable = true;
+
+ while (true) {
+ gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
+
+ // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
+ for (size_t i = 0; i < least_cost_poly->poly->edges.size(); i++) {
+ const gd::Edge &edge = least_cost_poly->poly->edges[i];
+
+ // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
+ for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
+ const gd::Edge::Connection &connection = edge.connections[connection_index];
+
+ // Only consider the connection to another polygon if this polygon is in a region with compatible layers.
+ if ((p_layers & connection.polygon->owner->get_layers()) == 0) {
+ continue;
+ }
+
+ Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end };
+ const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly->entry, pathway);
+ const float new_distance = least_cost_poly->entry.distance_to(new_entry) + least_cost_poly->traveled_distance;
+
+ const std::vector<gd::NavigationPoly>::iterator it = std::find(
+ navigation_polys.begin(),
+ navigation_polys.end(),
+ gd::NavigationPoly(connection.polygon));
+
+ if (it != navigation_polys.end()) {
+ // Polygon already visited, check if we can reduce the travel cost.
+ if (new_distance < it->traveled_distance) {
+ it->back_navigation_poly_id = least_cost_id;
+ it->back_navigation_edge = connection.edge;
+ it->back_navigation_edge_pathway_start = connection.pathway_start;
+ it->back_navigation_edge_pathway_end = connection.pathway_end;
+ it->traveled_distance = new_distance;
+ it->entry = new_entry;
+ }
+ } else {
+ // Add the neighbour polygon to the reachable ones.
+ gd::NavigationPoly new_navigation_poly = gd::NavigationPoly(connection.polygon);
+ new_navigation_poly.self_id = navigation_polys.size();
+ new_navigation_poly.back_navigation_poly_id = least_cost_id;
+ new_navigation_poly.back_navigation_edge = connection.edge;
+ new_navigation_poly.back_navigation_edge_pathway_start = connection.pathway_start;
+ new_navigation_poly.back_navigation_edge_pathway_end = connection.pathway_end;
+ new_navigation_poly.traveled_distance = new_distance;
+ new_navigation_poly.entry = new_entry;
+ navigation_polys.push_back(new_navigation_poly);
+
+ // Add the neighbour polygon to the polygons to visit.
+ to_visit.push_back(navigation_polys.size() - 1);
+ }
+ }
+ }
+
+ // Removes the least cost polygon from the list of polygons to visit so we can advance.
+ to_visit.erase(least_cost_id);
+
+ // When the list of polygons to visit is empty at this point it means the End Polygon is not reachable
+ if (to_visit.size() == 0) {
+ // Thus use the further reachable polygon
+ ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons");
+ is_reachable = false;
+ if (reachable_end == nullptr) {
+ // The path is not found and there is not a way out.
+ break;
+ }
+
+ // Set as end point the furthest reachable point.
+ end_poly = reachable_end;
+ end_d = 1e20;
+ for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
+ Face3 f(end_poly->points[point_id - 2].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
+ Vector3 spoint = f.get_closest_point_to(p_destination);
+ float dpoint = spoint.distance_to(p_destination);
+ if (dpoint < end_d) {
+ end_point = spoint;
+ end_d = dpoint;
+ }
+ }
+
+ // Reset open and navigation_polys
+ gd::NavigationPoly np = navigation_polys[0];
+ navigation_polys.clear();
+ navigation_polys.push_back(np);
+ to_visit.clear();
+ to_visit.push_back(0);
+
+ reachable_end = nullptr;
+
+ continue;
+ }
+
+ // Find the polygon with the minimum cost from the list of polygons to visit.
+ least_cost_id = -1;
+ float least_cost = 1e30;
+ for (List<uint32_t>::Element *element = to_visit.front(); element != nullptr; element = element->next()) {
+ gd::NavigationPoly *np = &navigation_polys[element->get()];
+ float cost = np->traveled_distance;
+ cost += np->entry.distance_to(end_point);
+ if (cost < least_cost) {
+ least_cost_id = np->self_id;
+ least_cost = cost;
+ }
+ }
+
+ // Stores the further reachable end polygon, in case our goal is not reachable.
+ if (is_reachable) {
+ float d = navigation_polys[least_cost_id].entry.distance_to(p_destination);
+ if (reachable_d > d) {
+ reachable_d = d;
+ reachable_end = navigation_polys[least_cost_id].poly;
+ }
+ }
+
+ ERR_BREAK(least_cost_id == -1);
+
+ // Check if we reached the end
+ if (navigation_polys[least_cost_id].poly == end_poly) {
+ found_route = true;
+ break;
+ }
+ }
+
+ // If we did not find a route, return an empty path.
+ if (!found_route) {
+ return Vector<Vector3>();
+ }
+
+ Vector<Vector3> path;
+ // Optimize the path.
+ if (p_optimize) {
+ // Set the apex poly/point to the end point
+ gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
+ Vector3 apex_point = end_point;
+
+ gd::NavigationPoly *left_poly = apex_poly;
+ Vector3 left_portal = apex_point;
+ gd::NavigationPoly *right_poly = apex_poly;
+ Vector3 right_portal = apex_point;
+
+ gd::NavigationPoly *p = apex_poly;
+
+ path.push_back(end_point);
+
+ while (p) {
+ // Set left and right points of the pathway between polygons.
+ Vector3 left = p->back_navigation_edge_pathway_start;
+ Vector3 right = p->back_navigation_edge_pathway_end;
+ if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(up) < 0) {
+ SWAP(left, right);
+ }
+
+ bool skip = false;
+ if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(up) >= 0) {
+ //process
+ if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(up) > 0) {
+ left_poly = p;
+ left_portal = left;
+ } else {
+ clip_path(navigation_polys, path, apex_poly, right_portal, right_poly);
+
+ apex_point = right_portal;
+ p = right_poly;
+ left_poly = p;
+ apex_poly = p;
+ left_portal = apex_point;
+ right_portal = apex_point;
+ path.push_back(apex_point);
+ skip = true;
+ }
+ }
+
+ if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(up) <= 0) {
+ //process
+ if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(up) < 0) {
+ right_poly = p;
+ right_portal = right;
+ } else {
+ clip_path(navigation_polys, path, apex_poly, left_portal, left_poly);
+
+ apex_point = left_portal;
+ p = left_poly;
+ right_poly = p;
+ apex_poly = p;
+ right_portal = apex_point;
+ left_portal = apex_point;
+ path.push_back(apex_point);
+ }
+ }
+
+ // Go to the previous polygon.
+ if (p->back_navigation_poly_id != -1) {
+ p = &navigation_polys[p->back_navigation_poly_id];
+ } else {
+ // The end
+ p = nullptr;
+ }
+ }
+
+ // If the last point is not the begin point, add it to the list.
+ if (path[path.size() - 1] != begin_point) {
+ path.push_back(begin_point);
+ }
+
+ path.reverse();
+
+ } else {
+ path.push_back(end_point);
+
+ // Add mid points
+ int np_id = least_cost_id;
+ while (np_id != -1) {
+ path.push_back(navigation_polys[np_id].entry);
+ np_id = navigation_polys[np_id].back_navigation_poly_id;
+ }
+
+ path.reverse();
+ }
+
+ return path;
+}
+
+Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
+ bool use_collision = p_use_collision;
+ Vector3 closest_point;
+ real_t closest_point_d = 1e20;
+
+ // Find the initial poly and the end poly on this map.
+ for (size_t i(0); i < polygons.size(); i++) {
+ const gd::Polygon &p = polygons[i];
+
+ // For each point cast a face and check the distance to the segment
+ for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
+ const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+ Vector3 inters;
+ if (f.intersects_segment(p_from, p_to, &inters)) {
+ const real_t d = closest_point_d = p_from.distance_to(inters);
+ if (use_collision == false) {
+ closest_point = inters;
+ use_collision = true;
+ closest_point_d = d;
+ } else if (closest_point_d > d) {
+ closest_point = inters;
+ closest_point_d = d;
+ }
+ }
+ }
+
+ if (use_collision == false) {
+ for (size_t point_id = 0; point_id < p.points.size(); point_id += 1) {
+ Vector3 a, b;
+
+ Geometry3D::get_closest_points_between_segments(
+ p_from,
+ p_to,
+ p.points[point_id].pos,
+ p.points[(point_id + 1) % p.points.size()].pos,
+ a,
+ b);
+
+ const real_t d = a.distance_to(b);
+ if (d < closest_point_d) {
+ closest_point_d = d;
+ closest_point = b;
+ }
+ }
+ }
+ }
+
+ return closest_point;
+}
+
+Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
+ // TODO this is really not optimal, please redesign the API to directly return all this data
+
+ Vector3 closest_point;
+ real_t closest_point_d = 1e20;
+
+ // Find the initial poly and the end poly on this map.
+ for (size_t i(0); i < polygons.size(); i++) {
+ const gd::Polygon &p = polygons[i];
+
+ // For each point cast a face and check the distance to the point
+ for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
+ const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+ const Vector3 inters = f.get_closest_point_to(p_point);
+ const real_t d = inters.distance_to(p_point);
+ if (d < closest_point_d) {
+ closest_point = inters;
+ closest_point_d = d;
+ }
+ }
+ }
+
+ return closest_point;
+}
+
+Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
+ // TODO this is really not optimal, please redesign the API to directly return all this data
+
+ Vector3 closest_point;
+ Vector3 closest_point_normal;
+ real_t closest_point_d = 1e20;
+
+ // Find the initial poly and the end poly on this map.
+ for (size_t i(0); i < polygons.size(); i++) {
+ const gd::Polygon &p = polygons[i];
+
+ // For each point cast a face and check the distance to the point
+ for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
+ const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+ const Vector3 inters = f.get_closest_point_to(p_point);
+ const real_t d = inters.distance_to(p_point);
+ if (d < closest_point_d) {
+ closest_point = inters;
+ closest_point_normal = f.get_plane().normal;
+ closest_point_d = d;
+ }
+ }
+ }
+
+ return closest_point_normal;
+}
+
+RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
+ // TODO this is really not optimal, please redesign the API to directly return all this data
+
+ Vector3 closest_point;
+ RID closest_point_owner;
+ real_t closest_point_d = 1e20;
+
+ // Find the initial poly and the end poly on this map.
+ for (size_t i(0); i < polygons.size(); i++) {
+ const gd::Polygon &p = polygons[i];
+
+ // For each point cast a face and check the distance to the point
+ for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
+ const Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+ const Vector3 inters = f.get_closest_point_to(p_point);
+ const real_t d = inters.distance_to(p_point);
+ if (d < closest_point_d) {
+ closest_point = inters;
+ closest_point_owner = p.owner->get_self();
+ closest_point_d = d;
+ }
+ }
+ }
+
+ return closest_point_owner;
+}
+
+void NavMap::add_region(NavRegion *p_region) {
+ regions.push_back(p_region);
+ regenerate_links = true;
+}
+
+void NavMap::remove_region(NavRegion *p_region) {
+ const std::vector<NavRegion *>::iterator it = std::find(regions.begin(), regions.end(), p_region);
+ if (it != regions.end()) {
+ regions.erase(it);
+ regenerate_links = true;
+ }
+}
+
+bool NavMap::has_agent(RvoAgent *agent) const {
+ return std::find(agents.begin(), agents.end(), agent) != agents.end();
+}
+
+void NavMap::add_agent(RvoAgent *agent) {
+ if (!has_agent(agent)) {
+ agents.push_back(agent);
+ agents_dirty = true;
+ }
+}
+
+void NavMap::remove_agent(RvoAgent *agent) {
+ remove_agent_as_controlled(agent);
+ const std::vector<RvoAgent *>::iterator it = std::find(agents.begin(), agents.end(), agent);
+ if (it != agents.end()) {
+ agents.erase(it);
+ agents_dirty = true;
+ }
+}
+
+void NavMap::set_agent_as_controlled(RvoAgent *agent) {
+ const bool exist = std::find(controlled_agents.begin(), controlled_agents.end(), agent) != controlled_agents.end();
+ if (!exist) {
+ ERR_FAIL_COND(!has_agent(agent));
+ controlled_agents.push_back(agent);
+ }
+}
+
+void NavMap::remove_agent_as_controlled(RvoAgent *agent) {
+ const std::vector<RvoAgent *>::iterator it = std::find(controlled_agents.begin(), controlled_agents.end(), agent);
+ if (it != controlled_agents.end()) {
+ controlled_agents.erase(it);
+ }
+}
+
+void NavMap::sync() {
+ // Check if we need to update the links.
+ if (regenerate_polygons) {
+ for (size_t r(0); r < regions.size(); r++) {
+ regions[r]->scratch_polygons();
+ }
+ regenerate_links = true;
+ }
+
+ for (size_t r(0); r < regions.size(); r++) {
+ if (regions[r]->sync()) {
+ regenerate_links = true;
+ }
+ }
+
+ if (regenerate_links) {
+ // Remove regions connections.
+ for (size_t r(0); r < regions.size(); r++) {
+ regions[r]->get_connections().clear();
+ }
+
+ // Resize the polygon count.
+ int count = 0;
+ for (size_t r(0); r < regions.size(); r++) {
+ count += regions[r]->get_polygons().size();
+ }
+ polygons.resize(count);
+
+ // Copy all region polygons in the map.
+ count = 0;
+ for (size_t r(0); r < regions.size(); r++) {
+ std::copy(
+ regions[r]->get_polygons().data(),
+ regions[r]->get_polygons().data() + regions[r]->get_polygons().size(),
+ polygons.begin() + count);
+ count += regions[r]->get_polygons().size();
+ }
+
+ // Group all edges per key.
+ Map<gd::EdgeKey, Vector<gd::Edge::Connection>> connections;
+ for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) {
+ gd::Polygon &poly(polygons[poly_id]);
+
+ for (size_t p(0); p < poly.points.size(); p++) {
+ int next_point = (p + 1) % poly.points.size();
+ gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
+
+ Map<gd::EdgeKey, Vector<gd::Edge::Connection>>::Element *connection = connections.find(ek);
+ if (!connection) {
+ connections[ek] = Vector<gd::Edge::Connection>();
+ }
+ if (connections[ek].size() <= 1) {
+ // Add the polygon/edge tuple to this key.
+ gd::Edge::Connection new_connection;
+ new_connection.polygon = &poly;
+ new_connection.edge = p;
+ new_connection.pathway_start = poly.points[p].pos;
+ new_connection.pathway_end = poly.points[next_point].pos;
+ connections[ek].push_back(new_connection);
+ } else {
+ // The edge is already connected with another edge, skip.
+ ERR_PRINT("Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the current `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem.");
+ }
+ }
+ }
+
+ Vector<gd::Edge::Connection> free_edges;
+ for (Map<gd::EdgeKey, Vector<gd::Edge::Connection>>::Element *E = connections.front(); E; E = E->next()) {
+ if (E->get().size() == 2) {
+ // Connect edge that are shared in different polygons.
+ gd::Edge::Connection &c1 = E->get().write[0];
+ gd::Edge::Connection &c2 = E->get().write[1];
+ c1.polygon->edges[c1.edge].connections.push_back(c2);
+ c2.polygon->edges[c2.edge].connections.push_back(c1);
+ // Note: The pathway_start/end are full for those connection and do not need to be modified.
+ } else {
+ CRASH_COND_MSG(E->get().size() != 1, vformat("Number of connection != 1. Found: %d", E->get().size()));
+ free_edges.push_back(E->get()[0]);
+ }
+ }
+
+ // 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.
+ for (int i = 0; i < free_edges.size(); i++) {
+ const gd::Edge::Connection &free_edge = free_edges[i];
+ Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
+ Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos;
+
+ for (int j = 0; j < free_edges.size(); j++) {
+ const gd::Edge::Connection &other_edge = free_edges[j];
+ if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
+ continue;
+ }
+
+ Vector3 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;
+ float projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector.length_squared());
+ float projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector.length_squared());
+ if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
+ continue;
+ }
+
+ // Check if the two edges are close to each other enough and compute a pathway between the two regions.
+ Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
+ Vector3 other1;
+ if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) {
+ other1 = other_edge_p1;
+ } else {
+ other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
+ }
+ if ((self1 - other1).length() > edge_connection_margin) {
+ continue;
+ }
+
+ Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
+ Vector3 other2;
+ if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) {
+ other2 = other_edge_p2;
+ } else {
+ other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
+ }
+ if ((self2 - other2).length() > edge_connection_margin) {
+ continue;
+ }
+
+ // The edges can now be connected.
+ gd::Edge::Connection new_connection = other_edge;
+ new_connection.pathway_start = (self1 + other1) / 2.0;
+ new_connection.pathway_end = (self2 + other2) / 2.0;
+ free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
+
+ // Add the connection to the region_connection map.
+ free_edge.polygon->owner->get_connections().push_back(new_connection);
+ }
+ }
+
+ // Update the update ID.
+ map_update_id = (map_update_id + 1) % 9999999;
+ }
+
+ // Update agents tree.
+ if (agents_dirty) {
+ std::vector<RVO::Agent *> raw_agents;
+ raw_agents.reserve(agents.size());
+ for (size_t i(0); i < agents.size(); i++) {
+ raw_agents.push_back(agents[i]->get_agent());
+ }
+ rvo.buildAgentTree(raw_agents);
+ }
+
+ regenerate_polygons = false;
+ regenerate_links = false;
+ agents_dirty = false;
+}
+
+void NavMap::compute_single_step(uint32_t index, RvoAgent **agent) {
+ (*(agent + index))->get_agent()->computeNeighbors(&rvo);
+ (*(agent + index))->get_agent()->computeNewVelocity(deltatime);
+}
+
+void NavMap::step(real_t p_deltatime) {
+ deltatime = p_deltatime;
+ if (controlled_agents.size() > 0) {
+ thread_process_array(
+ controlled_agents.size(),
+ this,
+ &NavMap::compute_single_step,
+ controlled_agents.data());
+ }
+}
+
+void NavMap::dispatch_callbacks() {
+ for (int i(0); i < static_cast<int>(controlled_agents.size()); i++) {
+ controlled_agents[i]->dispatch_callback();
+ }
+}
+
+void NavMap::clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const {
+ Vector3 from = path[path.size() - 1];
+
+ if (from.distance_to(p_to_point) < CMP_EPSILON) {
+ return;
+ }
+ Plane cut_plane;
+ cut_plane.normal = (from - p_to_point).cross(up);
+ if (cut_plane.normal == Vector3()) {
+ return;
+ }
+ cut_plane.normal.normalize();
+ cut_plane.d = cut_plane.normal.dot(from);
+
+ while (from_poly != p_to_poly) {
+ Vector3 pathway_start = from_poly->back_navigation_edge_pathway_start;
+ Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end;
+
+ ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1);
+ from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id];
+
+ if (pathway_start.distance_to(pathway_end) > CMP_EPSILON) {
+ Vector3 inters;
+ if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) {
+ if (inters.distance_to(p_to_point) > CMP_EPSILON && inters.distance_to(path[path.size() - 1]) > CMP_EPSILON) {
+ path.push_back(inters);
+ }
+ }
+ }
+ }
+}
diff --git a/modules/navigation/nav_map.h b/modules/navigation/nav_map.h
new file mode 100644
index 0000000000..8e013a72eb
--- /dev/null
+++ b/modules/navigation/nav_map.h
@@ -0,0 +1,141 @@
+/*************************************************************************/
+/* nav_map.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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 RVO_SPACE_H
+#define RVO_SPACE_H
+
+#include "nav_rid.h"
+
+#include "core/math/math_defs.h"
+#include "core/templates/map.h"
+#include "nav_utils.h"
+#include <KdTree.h>
+
+/**
+ @author AndreaCatania
+*/
+
+class NavRegion;
+class RvoAgent;
+class NavRegion;
+
+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.
+ real_t cell_size = 0.3;
+
+ /// This value is used to detect the near edges to connect.
+ real_t edge_connection_margin = 5.0;
+
+ bool regenerate_polygons = true;
+ bool regenerate_links = true;
+
+ std::vector<NavRegion *> regions;
+
+ /// Map polygons
+ std::vector<gd::Polygon> polygons;
+
+ /// Rvo world
+ RVO::KdTree rvo;
+
+ /// Is agent array modified?
+ bool agents_dirty = false;
+
+ /// All the Agents (even the controlled one)
+ std::vector<RvoAgent *> agents;
+
+ /// Controlled agents
+ std::vector<RvoAgent *> controlled_agents;
+
+ /// Physics delta time
+ real_t deltatime = 0.0;
+
+ /// Change the id each time the map is updated.
+ uint32_t map_update_id = 0;
+
+public:
+ NavMap() {}
+
+ void set_up(Vector3 p_up);
+ Vector3 get_up() const {
+ return up;
+ }
+
+ void set_cell_size(float p_cell_size);
+ float get_cell_size() const {
+ return cell_size;
+ }
+
+ void set_edge_connection_margin(float p_edge_connection_margin);
+ float get_edge_connection_margin() const {
+ return edge_connection_margin;
+ }
+
+ gd::PointKey get_point_key(const Vector3 &p_pos) const;
+
+ Vector<Vector3> get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers = 1) const;
+ Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
+ Vector3 get_closest_point(const Vector3 &p_point) const;
+ Vector3 get_closest_point_normal(const Vector3 &p_point) const;
+ RID get_closest_point_owner(const Vector3 &p_point) const;
+
+ void add_region(NavRegion *p_region);
+ void remove_region(NavRegion *p_region);
+ const std::vector<NavRegion *> &get_regions() const {
+ return regions;
+ }
+
+ bool has_agent(RvoAgent *agent) const;
+ void add_agent(RvoAgent *agent);
+ void remove_agent(RvoAgent *agent);
+ const std::vector<RvoAgent *> &get_agents() const {
+ return agents;
+ }
+
+ void set_agent_as_controlled(RvoAgent *agent);
+ void remove_agent_as_controlled(RvoAgent *agent);
+
+ uint32_t get_map_update_id() const {
+ return map_update_id;
+ }
+
+ void sync();
+ void step(real_t p_deltatime);
+ void dispatch_callbacks();
+
+private:
+ void compute_single_step(uint32_t index, RvoAgent **agent);
+ void clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const;
+};
+
+#endif // RVO_SPACE_H
diff --git a/modules/navigation/nav_region.cpp b/modules/navigation/nav_region.cpp
new file mode 100644
index 0000000000..81b15a49f5
--- /dev/null
+++ b/modules/navigation/nav_region.cpp
@@ -0,0 +1,161 @@
+/*************************************************************************/
+/* nav_region.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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"
+
+/**
+ @author AndreaCatania
+*/
+
+void NavRegion::set_map(NavMap *p_map) {
+ map = p_map;
+ polygons_dirty = true;
+ if (!map) {
+ connections.clear();
+ }
+}
+
+void NavRegion::set_layers(uint32_t p_layers) {
+ layers = p_layers;
+}
+
+uint32_t NavRegion::get_layers() const {
+ return layers;
+}
+
+void NavRegion::set_transform(Transform3D p_transform) {
+ transform = p_transform;
+ polygons_dirty = true;
+}
+
+void NavRegion::set_mesh(Ref<NavigationMesh> p_mesh) {
+ mesh = p_mesh;
+ polygons_dirty = true;
+}
+
+int NavRegion::get_connections_count() const {
+ if (!map) {
+ return 0;
+ }
+ return connections.size();
+}
+
+Vector3 NavRegion::get_connection_pathway_start(int p_connection_id) const {
+ ERR_FAIL_COND_V(!map, Vector3());
+ ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3());
+ return connections[p_connection_id].pathway_start;
+}
+
+Vector3 NavRegion::get_connection_pathway_end(int p_connection_id) const {
+ ERR_FAIL_COND_V(!map, Vector3());
+ ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3());
+ return connections[p_connection_id].pathway_end;
+}
+
+bool NavRegion::sync() {
+ bool something_changed = polygons_dirty /* || something_dirty? */;
+
+ update_polygons();
+
+ return something_changed;
+}
+
+void NavRegion::update_polygons() {
+ if (!polygons_dirty) {
+ return;
+ }
+ polygons.clear();
+ polygons_dirty = false;
+
+ if (map == nullptr) {
+ return;
+ }
+
+ if (mesh.is_null()) {
+ return;
+ }
+
+ Vector<Vector3> vertices = mesh->get_vertices();
+ int len = vertices.size();
+ if (len == 0) {
+ return;
+ }
+
+ const Vector3 *vertices_r = vertices.ptr();
+
+ polygons.resize(mesh->get_polygon_count());
+
+ // Build
+ for (size_t i(0); i < polygons.size(); i++) {
+ gd::Polygon &p = polygons[i];
+ p.owner = this;
+
+ Vector<int> mesh_poly = mesh->get_polygon(i);
+ const int *indices = mesh_poly.ptr();
+ bool valid(true);
+ p.points.resize(mesh_poly.size());
+ p.edges.resize(mesh_poly.size());
+
+ Vector3 center;
+ float sum(0);
+
+ for (int j(0); j < mesh_poly.size(); j++) {
+ int idx = indices[j];
+ if (idx < 0 || idx >= len) {
+ valid = false;
+ break;
+ }
+
+ Vector3 point_position = transform.xform(vertices_r[idx]);
+ p.points[j].pos = point_position;
+ p.points[j].key = map->get_point_key(point_position);
+
+ center += point_position; // Composing the center of the polygon
+
+ if (j >= 2) {
+ Vector3 epa = transform.xform(vertices_r[indices[j - 2]]);
+ Vector3 epb = transform.xform(vertices_r[indices[j - 1]]);
+
+ sum += map->get_up().dot((epb - epa).cross(point_position - epa));
+ }
+ }
+
+ if (!valid) {
+ ERR_BREAK_MSG(!valid, "The navigation mesh set in this region is not valid!");
+ }
+
+ p.clockwise = sum > 0;
+ if (mesh_poly.size() != 0) {
+ p.center = center / float(mesh_poly.size());
+ }
+ }
+}
diff --git a/modules/navigation/nav_region.h b/modules/navigation/nav_region.h
new file mode 100644
index 0000000000..f8b067e638
--- /dev/null
+++ b/modules/navigation/nav_region.h
@@ -0,0 +1,101 @@
+/*************************************************************************/
+/* nav_region.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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 "scene/resources/navigation_mesh.h"
+
+#include "nav_rid.h"
+#include "nav_utils.h"
+#include <vector>
+
+/**
+ @author AndreaCatania
+*/
+
+class NavMap;
+class NavRegion;
+
+class NavRegion : public NavRid {
+ NavMap *map = nullptr;
+ Transform3D transform;
+ Ref<NavigationMesh> mesh;
+ uint32_t layers = 1;
+ Vector<gd::Edge::Connection> connections;
+
+ bool polygons_dirty = true;
+
+ /// Cache
+ std::vector<gd::Polygon> polygons;
+
+public:
+ NavRegion() {}
+
+ void scratch_polygons() {
+ polygons_dirty = true;
+ }
+
+ void set_map(NavMap *p_map);
+ NavMap *get_map() const {
+ return map;
+ }
+
+ void set_layers(uint32_t p_layers);
+ uint32_t get_layers() const;
+
+ void set_transform(Transform3D transform);
+ const Transform3D &get_transform() const {
+ return transform;
+ }
+
+ void set_mesh(Ref<NavigationMesh> p_mesh);
+ const Ref<NavigationMesh> get_mesh() const {
+ return mesh;
+ }
+
+ Vector<gd::Edge::Connection> &get_connections() {
+ return connections;
+ }
+ int get_connections_count() const;
+ Vector3 get_connection_pathway_start(int p_connection_id) const;
+ Vector3 get_connection_pathway_end(int p_connection_id) const;
+
+ std::vector<gd::Polygon> const &get_polygons() const {
+ return polygons;
+ }
+
+ bool sync();
+
+private:
+ void update_polygons();
+};
+
+#endif // NAV_REGION_H
diff --git a/modules/navigation/nav_rid.h b/modules/navigation/nav_rid.h
new file mode 100644
index 0000000000..a0a60a3643
--- /dev/null
+++ b/modules/navigation/nav_rid.h
@@ -0,0 +1,48 @@
+/*************************************************************************/
+/* nav_rid.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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"
+
+/**
+ @author AndreaCatania
+*/
+
+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
diff --git a/modules/navigation/nav_utils.h b/modules/navigation/nav_utils.h
new file mode 100644
index 0000000000..35da391eea
--- /dev/null
+++ b/modules/navigation/nav_utils.h
@@ -0,0 +1,140 @@
+/*************************************************************************/
+/* nav_utils.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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 <vector>
+
+/**
+ @author AndreaCatania
+*/
+
+class NavRegion;
+
+namespace gd {
+struct Polygon;
+
+union PointKey {
+ struct {
+ int64_t x : 21;
+ int64_t y : 22;
+ int64_t z : 21;
+ };
+
+ uint64_t key = 0;
+ bool operator<(const PointKey &p_key) const { return key < p_key.key; }
+};
+
+struct EdgeKey {
+ PointKey a;
+ PointKey b;
+
+ bool operator<(const EdgeKey &p_key) const {
+ return (a.key == p_key.a.key) ? (b.key < p_key.b.key) : (a.key < p_key.a.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 {
+ /// This edge ID
+ int this_edge = -1;
+
+ /// The gateway in the edge, as, in some case, the whole edge might not be navigable.
+ struct Connection {
+ Polygon *polygon = nullptr;
+ int edge = -1;
+ Vector3 pathway_start;
+ Vector3 pathway_end;
+ };
+ Vector<Connection> connections;
+};
+
+struct Polygon {
+ NavRegion *owner;
+
+ /// The points of this `Polygon`
+ std::vector<Point> points;
+
+ /// Are the points clockwise ?
+ bool clockwise;
+
+ /// The edges of this `Polygon`
+ std::vector<Edge> edges;
+
+ /// The center of this `Polygon`
+ Vector3 center;
+};
+
+struct NavigationPoly {
+ uint32_t self_id = 0;
+ /// This poly.
+ const Polygon *poly;
+
+ /// Those 4 variables are used to travel the path backwards.
+ int back_navigation_poly_id = -1;
+ uint32_t back_navigation_edge = UINT32_MAX;
+ Vector3 back_navigation_edge_pathway_start;
+ Vector3 back_navigation_edge_pathway_end;
+
+ /// The entry location of this poly.
+ Vector3 entry;
+ /// The distance to the destination.
+ float traveled_distance = 0.0;
+
+ NavigationPoly(const Polygon *p_poly) :
+ poly(p_poly) {}
+
+ bool operator==(const NavigationPoly &other) const {
+ return this->poly == other.poly;
+ }
+
+ bool operator!=(const NavigationPoly &other) const {
+ return !operator==(other);
+ }
+};
+
+} // namespace gd
+
+#endif // NAV_UTILS_H
diff --git a/modules/navigation/navigation_mesh_editor_plugin.cpp b/modules/navigation/navigation_mesh_editor_plugin.cpp
new file mode 100644
index 0000000000..aa9248d2a1
--- /dev/null
+++ b/modules/navigation/navigation_mesh_editor_plugin.cpp
@@ -0,0 +1,154 @@
+/*************************************************************************/
+/* navigation_mesh_editor_plugin.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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 TOOLS_ENABLED
+#include "navigation_mesh_editor_plugin.h"
+
+#include "core/io/marshalls.h"
+#include "core/io/resource_saver.h"
+#include "navigation_mesh_generator.h"
+#include "scene/3d/mesh_instance_3d.h"
+#include "scene/gui/box_container.h"
+
+void NavigationMeshEditor::_node_removed(Node *p_node) {
+ if (p_node == node) {
+ node = nullptr;
+
+ hide();
+ }
+}
+
+void NavigationMeshEditor::_notification(int p_option) {
+ if (p_option == NOTIFICATION_ENTER_TREE) {
+ button_bake->set_icon(get_theme_icon("Bake", "EditorIcons"));
+ button_reset->set_icon(get_theme_icon("Reload", "EditorIcons"));
+ }
+}
+
+void NavigationMeshEditor::_bake_pressed() {
+ button_bake->set_pressed(false);
+
+ ERR_FAIL_COND(!node);
+ if (!node->get_navigation_mesh().is_valid()) {
+ err_dialog->set_text(TTR("A NavigationMesh resource must be set or created for this node to work."));
+ err_dialog->popup_centered();
+ return;
+ }
+
+ NavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh());
+ NavigationMeshGenerator::get_singleton()->bake(node->get_navigation_mesh(), node);
+
+ node->update_gizmo();
+}
+
+void NavigationMeshEditor::_clear_pressed() {
+ if (node) {
+ NavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh());
+ }
+
+ button_bake->set_pressed(false);
+ bake_info->set_text("");
+
+ if (node) {
+ node->update_gizmo();
+ }
+}
+
+void NavigationMeshEditor::edit(NavigationRegion3D *p_nav_region) {
+ if (p_nav_region == nullptr || node == p_nav_region) {
+ return;
+ }
+
+ node = p_nav_region;
+}
+
+void NavigationMeshEditor::_bind_methods() {
+}
+
+NavigationMeshEditor::NavigationMeshEditor() {
+ bake_hbox = memnew(HBoxContainer);
+
+ button_bake = memnew(Button);
+ button_bake->set_flat(true);
+ bake_hbox->add_child(button_bake);
+ button_bake->set_toggle_mode(true);
+ button_bake->set_text(TTR("Bake NavMesh"));
+ button_bake->connect("pressed", callable_mp(this, &NavigationMeshEditor::_bake_pressed));
+
+ button_reset = memnew(Button);
+ button_reset->set_flat(true);
+ bake_hbox->add_child(button_reset);
+ // No button text, we only use a revert icon which is set when entering the tree.
+ button_reset->set_tooltip(TTR("Clear the navigation mesh."));
+ button_reset->connect("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(EditorNode *p_node) {
+ editor = p_node;
+ navigation_mesh_editor = memnew(NavigationMeshEditor);
+ editor->get_main_control()->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
diff --git a/modules/navigation/navigation_mesh_editor_plugin.h b/modules/navigation/navigation_mesh_editor_plugin.h
new file mode 100644
index 0000000000..c39269865b
--- /dev/null
+++ b/modules/navigation/navigation_mesh_editor_plugin.h
@@ -0,0 +1,88 @@
+/*************************************************************************/
+/* navigation_mesh_editor_plugin.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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/editor_node.h"
+#include "editor/editor_plugin.h"
+
+class NavigationRegion3D;
+
+class NavigationMeshEditor : public Control {
+ friend class NavigationMeshEditorPlugin;
+
+ GDCLASS(NavigationMeshEditor, Control);
+
+ AcceptDialog *err_dialog;
+
+ HBoxContainer *bake_hbox;
+ Button *button_bake;
+ Button *button_reset;
+ Label *bake_info;
+
+ NavigationRegion3D *node;
+
+ void _bake_pressed();
+ void _clear_pressed();
+
+protected:
+ void _node_removed(Node *p_node);
+ static void _bind_methods();
+ void _notification(int p_option);
+
+public:
+ void edit(NavigationRegion3D *p_nav_region);
+ NavigationMeshEditor();
+ ~NavigationMeshEditor();
+};
+
+class NavigationMeshEditorPlugin : public EditorPlugin {
+ GDCLASS(NavigationMeshEditorPlugin, EditorPlugin);
+
+ NavigationMeshEditor *navigation_mesh_editor;
+ EditorNode *editor;
+
+public:
+ virtual String get_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(EditorNode *p_node);
+ ~NavigationMeshEditorPlugin();
+};
+
+#endif
+
+#endif
diff --git a/modules/navigation/navigation_mesh_generator.cpp b/modules/navigation/navigation_mesh_generator.cpp
new file mode 100644
index 0000000000..0d8330c1da
--- /dev/null
+++ b/modules/navigation/navigation_mesh_generator.cpp
@@ -0,0 +1,583 @@
+/*************************************************************************/
+/* navigation_mesh_generator.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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 "core/math/convex_hull.h"
+#include "core/os/thread.h"
+#include "scene/3d/collision_shape_3d.h"
+#include "scene/3d/mesh_instance_3d.h"
+#include "scene/3d/physics_body_3d.h"
+#include "scene/resources/box_shape_3d.h"
+#include "scene/resources/capsule_shape_3d.h"
+#include "scene/resources/concave_polygon_shape_3d.h"
+#include "scene/resources/convex_polygon_shape_3d.h"
+#include "scene/resources/cylinder_shape_3d.h"
+#include "scene/resources/primitive_meshes.h"
+#include "scene/resources/shape_3d.h"
+#include "scene/resources/sphere_shape_3d.h"
+#include "scene/resources/world_margin_shape_3d.h"
+
+#include "modules/modules_enabled.gen.h"
+#ifdef TOOLS_ENABLED
+#include "editor/editor_node.h"
+#include "editor/editor_settings.h"
+#endif
+
+#ifdef MODULE_CSG_ENABLED
+#include "modules/csg/csg_shape.h"
+#endif
+#ifdef MODULE_GRIDMAP_ENABLED
+#include "modules/gridmap/grid_map.h"
+#endif
+
+NavigationMeshGenerator *NavigationMeshGenerator::singleton = nullptr;
+
+void NavigationMeshGenerator::_add_vertex(const Vector3 &p_vec3, Vector<float> &p_verticies) {
+ p_verticies.push_back(p_vec3.x);
+ p_verticies.push_back(p_vec3.y);
+ p_verticies.push_back(p_vec3.z);
+}
+
+void NavigationMeshGenerator::_add_mesh(const Ref<Mesh> &p_mesh, const Transform3D &p_xform, Vector<float> &p_verticies, Vector<int> &p_indices) {
+ int current_vertex_count;
+
+ for (int i = 0; i < p_mesh->get_surface_count(); i++) {
+ current_vertex_count = p_verticies.size() / 3;
+
+ if (p_mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) {
+ continue;
+ }
+
+ int index_count = 0;
+ if (p_mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
+ index_count = p_mesh->surface_get_array_index_len(i);
+ } else {
+ index_count = p_mesh->surface_get_array_len(i);
+ }
+
+ ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0));
+
+ int face_count = index_count / 3;
+
+ Array a = p_mesh->surface_get_arrays(i);
+
+ Vector<Vector3> mesh_vertices = a[Mesh::ARRAY_VERTEX];
+ const Vector3 *vr = mesh_vertices.ptr();
+
+ if (p_mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
+ Vector<int> mesh_indices = a[Mesh::ARRAY_INDEX];
+ const int *ir = mesh_indices.ptr();
+
+ for (int j = 0; j < mesh_vertices.size(); j++) {
+ _add_vertex(p_xform.xform(vr[j]), p_verticies);
+ }
+
+ for (int j = 0; j < face_count; j++) {
+ // CCW
+ p_indices.push_back(current_vertex_count + (ir[j * 3 + 0]));
+ p_indices.push_back(current_vertex_count + (ir[j * 3 + 2]));
+ p_indices.push_back(current_vertex_count + (ir[j * 3 + 1]));
+ }
+ } else {
+ face_count = mesh_vertices.size() / 3;
+ for (int j = 0; j < face_count; j++) {
+ _add_vertex(p_xform.xform(vr[j * 3 + 0]), p_verticies);
+ _add_vertex(p_xform.xform(vr[j * 3 + 2]), p_verticies);
+ _add_vertex(p_xform.xform(vr[j * 3 + 1]), p_verticies);
+
+ p_indices.push_back(current_vertex_count + (j * 3 + 0));
+ p_indices.push_back(current_vertex_count + (j * 3 + 1));
+ p_indices.push_back(current_vertex_count + (j * 3 + 2));
+ }
+ }
+ }
+}
+
+void NavigationMeshGenerator::_add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform, Vector<float> &p_verticies, Vector<int> &p_indices) {
+ int face_count = p_faces.size() / 3;
+ int current_vertex_count = p_verticies.size() / 3;
+
+ for (int j = 0; j < face_count; j++) {
+ _add_vertex(p_xform.xform(p_faces[j * 3 + 0]), p_verticies);
+ _add_vertex(p_xform.xform(p_faces[j * 3 + 1]), p_verticies);
+ _add_vertex(p_xform.xform(p_faces[j * 3 + 2]), p_verticies);
+
+ p_indices.push_back(current_vertex_count + (j * 3 + 0));
+ p_indices.push_back(current_vertex_count + (j * 3 + 2));
+ p_indices.push_back(current_vertex_count + (j * 3 + 1));
+ }
+}
+
+void NavigationMeshGenerator::_parse_geometry(Transform3D p_accumulated_transform, Node *p_node, Vector<float> &p_verticies, Vector<int> &p_indices, int p_generate_from, uint32_t p_collision_mask, bool p_recurse_children) {
+ if (Object::cast_to<MeshInstance3D>(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) {
+ MeshInstance3D *mesh_instance = Object::cast_to<MeshInstance3D>(p_node);
+ Ref<Mesh> mesh = mesh_instance->get_mesh();
+ if (mesh.is_valid()) {
+ _add_mesh(mesh, p_accumulated_transform * mesh_instance->get_transform(), p_verticies, p_indices);
+ }
+ }
+
+#ifdef MODULE_CSG_ENABLED
+ if (Object::cast_to<CSGShape3D>(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) {
+ CSGShape3D *csg_shape = Object::cast_to<CSGShape3D>(p_node);
+ Array meshes = csg_shape->get_meshes();
+ if (!meshes.is_empty()) {
+ Ref<Mesh> mesh = meshes[1];
+ if (mesh.is_valid()) {
+ _add_mesh(mesh, p_accumulated_transform * csg_shape->get_transform(), p_verticies, p_indices);
+ }
+ }
+ }
+#endif
+
+ if (Object::cast_to<StaticBody3D>(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES) {
+ StaticBody3D *static_body = Object::cast_to<StaticBody3D>(p_node);
+
+ if (static_body->get_collision_layer() & p_collision_mask) {
+ for (int i = 0; i < p_node->get_child_count(); ++i) {
+ Node *child = p_node->get_child(i);
+ if (Object::cast_to<CollisionShape3D>(child)) {
+ CollisionShape3D *col_shape = Object::cast_to<CollisionShape3D>(child);
+
+ Transform3D transform = p_accumulated_transform * static_body->get_transform() * col_shape->get_transform();
+
+ Ref<Mesh> mesh;
+ Ref<Shape3D> s = col_shape->get_shape();
+
+ BoxShape3D *box = Object::cast_to<BoxShape3D>(*s);
+ if (box) {
+ Ref<BoxMesh> box_mesh;
+ box_mesh.instantiate();
+ box_mesh->set_size(box->get_size());
+ mesh = box_mesh;
+ }
+
+ CapsuleShape3D *capsule = Object::cast_to<CapsuleShape3D>(*s);
+ if (capsule) {
+ Ref<CapsuleMesh> capsule_mesh;
+ capsule_mesh.instantiate();
+ capsule_mesh->set_radius(capsule->get_radius());
+ capsule_mesh->set_mid_height(capsule->get_height() / 2.0);
+ mesh = capsule_mesh;
+ }
+
+ CylinderShape3D *cylinder = Object::cast_to<CylinderShape3D>(*s);
+ if (cylinder) {
+ Ref<CylinderMesh> cylinder_mesh;
+ cylinder_mesh.instantiate();
+ cylinder_mesh->set_height(cylinder->get_height());
+ cylinder_mesh->set_bottom_radius(cylinder->get_radius());
+ cylinder_mesh->set_top_radius(cylinder->get_radius());
+ mesh = cylinder_mesh;
+ }
+
+ SphereShape3D *sphere = Object::cast_to<SphereShape3D>(*s);
+ if (sphere) {
+ Ref<SphereMesh> sphere_mesh;
+ sphere_mesh.instantiate();
+ sphere_mesh->set_radius(sphere->get_radius());
+ sphere_mesh->set_height(sphere->get_radius() * 2.0);
+ mesh = sphere_mesh;
+ }
+
+ ConcavePolygonShape3D *concave_polygon = Object::cast_to<ConcavePolygonShape3D>(*s);
+ if (concave_polygon) {
+ _add_faces(concave_polygon->get_faces(), transform, p_verticies, p_indices);
+ }
+
+ ConvexPolygonShape3D *convex_polygon = Object::cast_to<ConvexPolygonShape3D>(*s);
+ if (convex_polygon) {
+ Vector<Vector3> varr = Variant(convex_polygon->get_points());
+ Geometry3D::MeshData md;
+
+ Error err = ConvexHullComputer::convex_hull(varr, md);
+
+ if (err == OK) {
+ PackedVector3Array faces;
+
+ for (int j = 0; j < md.faces.size(); ++j) {
+ Geometry3D::MeshData::Face face = md.faces[j];
+
+ for (int k = 2; k < face.indices.size(); ++k) {
+ faces.push_back(md.vertices[face.indices[0]]);
+ faces.push_back(md.vertices[face.indices[k - 1]]);
+ faces.push_back(md.vertices[face.indices[k]]);
+ }
+ }
+
+ _add_faces(faces, transform, p_verticies, p_indices);
+ }
+ }
+
+ if (mesh.is_valid()) {
+ _add_mesh(mesh, transform, p_verticies, p_indices);
+ }
+ }
+ }
+ }
+ }
+
+#ifdef MODULE_GRIDMAP_ENABLED
+ if (Object::cast_to<GridMap>(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) {
+ GridMap *gridmap_instance = Object::cast_to<GridMap>(p_node);
+ Array meshes = gridmap_instance->get_meshes();
+ Transform3D xform = gridmap_instance->get_transform();
+ for (int i = 0; i < meshes.size(); i += 2) {
+ Ref<Mesh> mesh = meshes[i + 1];
+ if (mesh.is_valid()) {
+ _add_mesh(mesh, p_accumulated_transform * xform * (Transform3D)meshes[i], p_verticies, p_indices);
+ }
+ }
+ }
+#endif
+
+ if (Object::cast_to<Node3D>(p_node)) {
+ Node3D *spatial = Object::cast_to<Node3D>(p_node);
+ p_accumulated_transform = p_accumulated_transform * spatial->get_transform();
+ }
+
+ if (p_recurse_children) {
+ for (int i = 0; i < p_node->get_child_count(); i++) {
+ _parse_geometry(p_accumulated_transform, p_node->get_child(i), p_verticies, p_indices, p_generate_from, p_collision_mask, p_recurse_children);
+ }
+ }
+}
+
+void NavigationMeshGenerator::_convert_detail_mesh_to_native_navigation_mesh(const rcPolyMeshDetail *p_detail_mesh, Ref<NavigationMesh> p_nav_mesh) {
+ Vector<Vector3> nav_vertices;
+
+ for (int i = 0; i < p_detail_mesh->nverts; i++) {
+ const float *v = &p_detail_mesh->verts[i * 3];
+ nav_vertices.push_back(Vector3(v[0], v[1], v[2]));
+ }
+ p_nav_mesh->set_vertices(nav_vertices);
+
+ for (int i = 0; i < p_detail_mesh->nmeshes; i++) {
+ const unsigned int *m = &p_detail_mesh->meshes[i * 4];
+ const unsigned int bverts = m[0];
+ const unsigned int btris = m[2];
+ const unsigned int ntris = m[3];
+ const unsigned char *tris = &p_detail_mesh->tris[btris * 4];
+ for (unsigned int j = 0; j < ntris; j++) {
+ Vector<int> nav_indices;
+ nav_indices.resize(3);
+ // Polygon order in recast is opposite than godot's
+ nav_indices.write[0] = ((int)(bverts + tris[j * 4 + 0]));
+ nav_indices.write[1] = ((int)(bverts + tris[j * 4 + 2]));
+ nav_indices.write[2] = ((int)(bverts + tris[j * 4 + 1]));
+ p_nav_mesh->add_polygon(nav_indices);
+ }
+ }
+}
+
+void NavigationMeshGenerator::_build_recast_navigation_mesh(
+ Ref<NavigationMesh> p_nav_mesh,
+#ifdef TOOLS_ENABLED
+ EditorProgress *ep,
+#endif
+ rcHeightfield *hf,
+ rcCompactHeightfield *chf,
+ rcContourSet *cset,
+ rcPolyMesh *poly_mesh,
+ rcPolyMeshDetail *detail_mesh,
+ Vector<float> &vertices,
+ Vector<int> &indices) {
+ rcContext ctx;
+
+#ifdef TOOLS_ENABLED
+ if (ep) {
+ ep->step(TTR("Setting up Configuration..."), 1);
+ }
+#endif
+
+ const float *verts = vertices.ptr();
+ const int nverts = vertices.size() / 3;
+ const int *tris = indices.ptr();
+ const int ntris = indices.size() / 3;
+
+ float bmin[3], bmax[3];
+ rcCalcBounds(verts, nverts, bmin, bmax);
+
+ rcConfig cfg;
+ memset(&cfg, 0, sizeof(cfg));
+
+ cfg.cs = p_nav_mesh->get_cell_size();
+ cfg.ch = p_nav_mesh->get_cell_height();
+ cfg.walkableSlopeAngle = p_nav_mesh->get_agent_max_slope();
+ cfg.walkableHeight = (int)Math::ceil(p_nav_mesh->get_agent_height() / cfg.ch);
+ cfg.walkableClimb = (int)Math::floor(p_nav_mesh->get_agent_max_climb() / cfg.ch);
+ cfg.walkableRadius = (int)Math::ceil(p_nav_mesh->get_agent_radius() / cfg.cs);
+ cfg.maxEdgeLen = (int)(p_nav_mesh->get_edge_max_length() / p_nav_mesh->get_cell_size());
+ cfg.maxSimplificationError = p_nav_mesh->get_edge_max_error();
+ cfg.minRegionArea = (int)(p_nav_mesh->get_region_min_size() * p_nav_mesh->get_region_min_size());
+ cfg.mergeRegionArea = (int)(p_nav_mesh->get_region_merge_size() * p_nav_mesh->get_region_merge_size());
+ cfg.maxVertsPerPoly = (int)p_nav_mesh->get_verts_per_poly();
+ cfg.detailSampleDist = p_nav_mesh->get_detail_sample_distance() < 0.9f ? 0 : p_nav_mesh->get_cell_size() * p_nav_mesh->get_detail_sample_distance();
+ cfg.detailSampleMaxError = p_nav_mesh->get_cell_height() * p_nav_mesh->get_detail_sample_max_error();
+
+ 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];
+
+#ifdef TOOLS_ENABLED
+ if (ep) {
+ ep->step(TTR("Calculating grid size..."), 2);
+ }
+#endif
+ rcCalcGridSize(cfg.bmin, cfg.bmax, cfg.cs, &cfg.width, &cfg.height);
+
+#ifdef TOOLS_ENABLED
+ if (ep) {
+ ep->step(TTR("Creating heightfield..."), 3);
+ }
+#endif
+ hf = rcAllocHeightfield();
+
+ ERR_FAIL_COND(!hf);
+ ERR_FAIL_COND(!rcCreateHeightfield(&ctx, *hf, cfg.width, cfg.height, cfg.bmin, cfg.bmax, cfg.cs, cfg.ch));
+
+#ifdef TOOLS_ENABLED
+ if (ep) {
+ ep->step(TTR("Marking walkable triangles..."), 4);
+ }
+#endif
+ {
+ Vector<unsigned char> tri_areas;
+ tri_areas.resize(ntris);
+
+ ERR_FAIL_COND(tri_areas.size() == 0);
+
+ 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_nav_mesh->get_filter_low_hanging_obstacles()) {
+ rcFilterLowHangingWalkableObstacles(&ctx, cfg.walkableClimb, *hf);
+ }
+ if (p_nav_mesh->get_filter_ledge_spans()) {
+ rcFilterLedgeSpans(&ctx, cfg.walkableHeight, cfg.walkableClimb, *hf);
+ }
+ if (p_nav_mesh->get_filter_walkable_low_height_spans()) {
+ rcFilterWalkableLowHeightSpans(&ctx, cfg.walkableHeight, *hf);
+ }
+
+#ifdef TOOLS_ENABLED
+ if (ep) {
+ ep->step(TTR("Constructing compact heightfield..."), 5);
+ }
+#endif
+
+ chf = rcAllocCompactHeightfield();
+
+ ERR_FAIL_COND(!chf);
+ ERR_FAIL_COND(!rcBuildCompactHeightfield(&ctx, cfg.walkableHeight, cfg.walkableClimb, *hf, *chf));
+
+ rcFreeHeightField(hf);
+ hf = nullptr;
+
+#ifdef TOOLS_ENABLED
+ if (ep) {
+ ep->step(TTR("Eroding walkable area..."), 6);
+ }
+#endif
+
+ ERR_FAIL_COND(!rcErodeWalkableArea(&ctx, cfg.walkableRadius, *chf));
+
+#ifdef TOOLS_ENABLED
+ if (ep) {
+ ep->step(TTR("Partitioning..."), 7);
+ }
+#endif
+
+ if (p_nav_mesh->get_sample_partition_type() == NavigationMesh::SAMPLE_PARTITION_WATERSHED) {
+ ERR_FAIL_COND(!rcBuildDistanceField(&ctx, *chf));
+ ERR_FAIL_COND(!rcBuildRegions(&ctx, *chf, 0, cfg.minRegionArea, cfg.mergeRegionArea));
+ } else if (p_nav_mesh->get_sample_partition_type() == NavigationMesh::SAMPLE_PARTITION_MONOTONE) {
+ ERR_FAIL_COND(!rcBuildRegionsMonotone(&ctx, *chf, 0, cfg.minRegionArea, cfg.mergeRegionArea));
+ } else {
+ ERR_FAIL_COND(!rcBuildLayerRegions(&ctx, *chf, 0, cfg.minRegionArea));
+ }
+
+#ifdef TOOLS_ENABLED
+ if (ep) {
+ ep->step(TTR("Creating contours..."), 8);
+ }
+#endif
+
+ cset = rcAllocContourSet();
+
+ ERR_FAIL_COND(!cset);
+ ERR_FAIL_COND(!rcBuildContours(&ctx, *chf, cfg.maxSimplificationError, cfg.maxEdgeLen, *cset));
+
+#ifdef TOOLS_ENABLED
+ if (ep) {
+ ep->step(TTR("Creating polymesh..."), 9);
+ }
+#endif
+
+ poly_mesh = rcAllocPolyMesh();
+ ERR_FAIL_COND(!poly_mesh);
+ ERR_FAIL_COND(!rcBuildPolyMesh(&ctx, *cset, cfg.maxVertsPerPoly, *poly_mesh));
+
+ detail_mesh = rcAllocPolyMeshDetail();
+ ERR_FAIL_COND(!detail_mesh);
+ ERR_FAIL_COND(!rcBuildPolyMeshDetail(&ctx, *poly_mesh, *chf, cfg.detailSampleDist, cfg.detailSampleMaxError, *detail_mesh));
+
+ rcFreeCompactHeightfield(chf);
+ chf = nullptr;
+ rcFreeContourSet(cset);
+ cset = nullptr;
+
+#ifdef TOOLS_ENABLED
+ if (ep) {
+ ep->step(TTR("Converting to native navigation mesh..."), 10);
+ }
+#endif
+
+ _convert_detail_mesh_to_native_navigation_mesh(detail_mesh, p_nav_mesh);
+
+ rcFreePolyMesh(poly_mesh);
+ poly_mesh = nullptr;
+ rcFreePolyMeshDetail(detail_mesh);
+ detail_mesh = nullptr;
+}
+
+NavigationMeshGenerator *NavigationMeshGenerator::get_singleton() {
+ return singleton;
+}
+
+NavigationMeshGenerator::NavigationMeshGenerator() {
+ singleton = this;
+}
+
+NavigationMeshGenerator::~NavigationMeshGenerator() {
+}
+
+void NavigationMeshGenerator::bake(Ref<NavigationMesh> p_nav_mesh, Node *p_node) {
+ ERR_FAIL_COND(!p_nav_mesh.is_valid());
+
+#ifdef TOOLS_ENABLED
+ EditorProgress *ep(nullptr);
+ if (Engine::get_singleton()->is_editor_hint()) {
+ ep = memnew(EditorProgress("bake", TTR("Navigation Mesh Generator Setup:"), 11));
+ }
+
+ if (ep) {
+ ep->step(TTR("Parsing Geometry..."), 0);
+ }
+#endif
+
+ Vector<float> vertices;
+ Vector<int> indices;
+
+ List<Node *> parse_nodes;
+
+ if (p_nav_mesh->get_source_geometry_mode() == NavigationMesh::SOURCE_GEOMETRY_NAVMESH_CHILDREN) {
+ parse_nodes.push_back(p_node);
+ } else {
+ p_node->get_tree()->get_nodes_in_group(p_nav_mesh->get_source_group_name(), &parse_nodes);
+ }
+
+ Transform3D navmesh_xform = Object::cast_to<Node3D>(p_node)->get_transform().affine_inverse();
+ for (const List<Node *>::Element *E = parse_nodes.front(); E; E = E->next()) {
+ int geometry_type = p_nav_mesh->get_parsed_geometry_type();
+ uint32_t collision_mask = p_nav_mesh->get_collision_mask();
+ bool recurse_children = p_nav_mesh->get_source_geometry_mode() != NavigationMesh::SOURCE_GEOMETRY_GROUPS_EXPLICIT;
+ _parse_geometry(navmesh_xform, E->get(), vertices, indices, geometry_type, collision_mask, recurse_children);
+ }
+
+ if (vertices.size() > 0 && indices.size() > 0) {
+ rcHeightfield *hf = nullptr;
+ rcCompactHeightfield *chf = nullptr;
+ rcContourSet *cset = nullptr;
+ rcPolyMesh *poly_mesh = nullptr;
+ rcPolyMeshDetail *detail_mesh = nullptr;
+
+ _build_recast_navigation_mesh(
+ p_nav_mesh,
+#ifdef TOOLS_ENABLED
+ ep,
+#endif
+ hf,
+ chf,
+ cset,
+ poly_mesh,
+ detail_mesh,
+ vertices,
+ indices);
+
+ rcFreeHeightField(hf);
+ hf = nullptr;
+
+ rcFreeCompactHeightfield(chf);
+ chf = nullptr;
+
+ rcFreeContourSet(cset);
+ cset = nullptr;
+
+ rcFreePolyMesh(poly_mesh);
+ poly_mesh = nullptr;
+
+ rcFreePolyMeshDetail(detail_mesh);
+ detail_mesh = nullptr;
+ }
+
+#ifdef TOOLS_ENABLED
+ if (ep) {
+ ep->step(TTR("Done!"), 11);
+ }
+
+ if (ep) {
+ memdelete(ep);
+ }
+#endif
+}
+
+void NavigationMeshGenerator::clear(Ref<NavigationMesh> p_nav_mesh) {
+ if (p_nav_mesh.is_valid()) {
+ p_nav_mesh->clear_polygons();
+ p_nav_mesh->set_vertices(Vector<Vector3>());
+ }
+}
+
+void NavigationMeshGenerator::_bind_methods() {
+ ClassDB::bind_method(D_METHOD("bake", "nav_mesh", "root_node"), &NavigationMeshGenerator::bake);
+ ClassDB::bind_method(D_METHOD("clear", "nav_mesh"), &NavigationMeshGenerator::clear);
+}
+
+#endif
diff --git a/modules/navigation/navigation_mesh_generator.h b/modules/navigation/navigation_mesh_generator.h
new file mode 100644
index 0000000000..847c7d097b
--- /dev/null
+++ b/modules/navigation/navigation_mesh_generator.h
@@ -0,0 +1,83 @@
+/*************************************************************************/
+/* navigation_mesh_generator.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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 <Recast.h>
+
+#ifdef TOOLS_ENABLED
+struct EditorProgress;
+#endif
+
+class NavigationMeshGenerator : public Object {
+ GDCLASS(NavigationMeshGenerator, Object);
+
+ static NavigationMeshGenerator *singleton;
+
+protected:
+ static void _bind_methods();
+
+ static void _add_vertex(const Vector3 &p_vec3, Vector<float> &p_verticies);
+ static void _add_mesh(const Ref<Mesh> &p_mesh, const Transform3D &p_xform, Vector<float> &p_verticies, Vector<int> &p_indices);
+ static void _add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform, Vector<float> &p_verticies, Vector<int> &p_indices);
+ static void _parse_geometry(Transform3D p_accumulated_transform, Node *p_node, Vector<float> &p_verticies, Vector<int> &p_indices, int p_generate_from, uint32_t p_collision_mask, bool p_recurse_children);
+
+ static void _convert_detail_mesh_to_native_navigation_mesh(const rcPolyMeshDetail *p_detail_mesh, Ref<NavigationMesh> p_nav_mesh);
+ static void _build_recast_navigation_mesh(
+ Ref<NavigationMesh> p_nav_mesh,
+#ifdef TOOLS_ENABLED
+ EditorProgress *ep,
+#endif
+ rcHeightfield *hf,
+ rcCompactHeightfield *chf,
+ rcContourSet *cset,
+ rcPolyMesh *poly_mesh,
+ rcPolyMeshDetail *detail_mesh,
+ Vector<float> &vertices,
+ Vector<int> &indices);
+
+public:
+ static NavigationMeshGenerator *get_singleton();
+
+ NavigationMeshGenerator();
+ ~NavigationMeshGenerator();
+
+ void bake(Ref<NavigationMesh> p_nav_mesh, Node *p_node);
+ void clear(Ref<NavigationMesh> p_nav_mesh);
+};
+
+#endif
+
+#endif // NAVIGATION_MESH_GENERATOR_H
diff --git a/modules/navigation/register_types.cpp b/modules/navigation/register_types.cpp
new file mode 100644
index 0000000000..0f3c412d4a
--- /dev/null
+++ b/modules/navigation/register_types.cpp
@@ -0,0 +1,74 @@
+/*************************************************************************/
+/* register_types.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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 "core/config/engine.h"
+#include "servers/navigation_server_3d.h"
+
+#include "godot_navigation_server.h"
+
+#ifndef _3D_DISABLED
+#include "navigation_mesh_generator.h"
+#endif
+
+#ifdef TOOLS_ENABLED
+#include "navigation_mesh_editor_plugin.h"
+#endif
+
+#ifndef _3D_DISABLED
+NavigationMeshGenerator *_nav_mesh_generator = nullptr;
+#endif
+
+NavigationServer3D *new_server() {
+ return memnew(GodotNavigationServer);
+}
+
+void register_navigation_types() {
+ NavigationServer3DManager::set_default_server(new_server);
+
+#ifndef _3D_DISABLED
+ _nav_mesh_generator = memnew(NavigationMeshGenerator);
+ ClassDB::register_class<NavigationMeshGenerator>();
+ Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationMeshGenerator", NavigationMeshGenerator::get_singleton()));
+#endif
+
+#ifdef TOOLS_ENABLED
+ EditorPlugins::add_by_type<NavigationMeshEditorPlugin>();
+#endif
+}
+
+void unregister_navigation_types() {
+#ifndef _3D_DISABLED
+ if (_nav_mesh_generator) {
+ memdelete(_nav_mesh_generator);
+ }
+#endif
+}
diff --git a/modules/navigation/register_types.h b/modules/navigation/register_types.h
new file mode 100644
index 0000000000..4737c818eb
--- /dev/null
+++ b/modules/navigation/register_types.h
@@ -0,0 +1,37 @@
+/*************************************************************************/
+/* register_types.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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
+
+void register_navigation_types();
+void unregister_navigation_types();
+
+#endif // NAVIGATION_REGISTER_TYPES_H
diff --git a/modules/navigation/rvo_agent.cpp b/modules/navigation/rvo_agent.cpp
new file mode 100644
index 0000000000..21e43d08c1
--- /dev/null
+++ b/modules/navigation/rvo_agent.cpp
@@ -0,0 +1,83 @@
+/*************************************************************************/
+/* rvo_agent.cpp */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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 "rvo_agent.h"
+
+#include "nav_map.h"
+
+/**
+ @author AndreaCatania
+*/
+
+RvoAgent::RvoAgent() {
+ callback.id = ObjectID();
+}
+
+void RvoAgent::set_map(NavMap *p_map) {
+ map = p_map;
+}
+
+bool RvoAgent::is_map_changed() {
+ if (map) {
+ bool is_changed = map->get_map_update_id() != map_update_id;
+ map_update_id = map->get_map_update_id();
+ return is_changed;
+ } else {
+ return false;
+ }
+}
+
+void RvoAgent::set_callback(ObjectID p_id, const StringName p_method, const Variant p_udata) {
+ callback.id = p_id;
+ callback.method = p_method;
+ callback.udata = p_udata;
+}
+
+bool RvoAgent::has_callback() const {
+ return callback.id.is_valid();
+}
+
+void RvoAgent::dispatch_callback() {
+ if (callback.id.is_null()) {
+ return;
+ }
+ Object *obj = ObjectDB::get_instance(callback.id);
+ if (obj == nullptr) {
+ callback.id = ObjectID();
+ }
+
+ Callable::CallError responseCallError;
+
+ callback.new_velocity = Vector3(agent.newVelocity_.x(), agent.newVelocity_.y(), agent.newVelocity_.z());
+
+ const Variant *vp[2] = { &callback.new_velocity, &callback.udata };
+ int argc = (callback.udata.get_type() == Variant::NIL) ? 1 : 2;
+ obj->call(callback.method, vp, argc, responseCallError);
+}
diff --git a/modules/navigation/rvo_agent.h b/modules/navigation/rvo_agent.h
new file mode 100644
index 0000000000..369cb1f9a3
--- /dev/null
+++ b/modules/navigation/rvo_agent.h
@@ -0,0 +1,78 @@
+/*************************************************************************/
+/* rvo_agent.h */
+/*************************************************************************/
+/* This file is part of: */
+/* GODOT ENGINE */
+/* https://godotengine.org */
+/*************************************************************************/
+/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2014-2021 Godot Engine contributors (cf. AUTHORS.md). */
+/* */
+/* 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 RVO_AGENT_H
+#define RVO_AGENT_H
+
+#include "core/object/class_db.h"
+#include "nav_rid.h"
+
+#include <Agent.h>
+
+/**
+ @author AndreaCatania
+*/
+
+class NavMap;
+
+class RvoAgent : public NavRid {
+ struct AvoidanceComputedCallback {
+ ObjectID id;
+ StringName method;
+ Variant udata;
+ Variant new_velocity;
+ };
+
+ NavMap *map = nullptr;
+ RVO::Agent agent;
+ AvoidanceComputedCallback callback;
+ uint32_t map_update_id = 0;
+
+public:
+ RvoAgent();
+
+ void set_map(NavMap *p_map);
+ NavMap *get_map() {
+ return map;
+ }
+
+ RVO::Agent *get_agent() {
+ return &agent;
+ }
+
+ bool is_map_changed();
+
+ void set_callback(ObjectID p_id, const StringName p_method, const Variant p_udata = Variant());
+ bool has_callback() const;
+
+ void dispatch_callback();
+};
+
+#endif // RVO_AGENT_H
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-26 11:50:29 +0000