From 74dc5e27c88adc2014e205030604497ca3334b2d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=A9mi=20Verschelde?= Date: Wed, 30 Jun 2021 00:16:35 +0200 Subject: Rename `GdNavigationServer` to `GodotNavigationServer` And rename `gdnavigation` module to simply `navigation`. --- modules/gdnavigation/SCsub | 63 -- modules/gdnavigation/config.py | 6 - modules/gdnavigation/gd_navigation_server.cpp | 547 --------------- modules/gdnavigation/gd_navigation_server.h | 149 ---- modules/gdnavigation/nav_map.cpp | 764 --------------------- modules/gdnavigation/nav_map.h | 141 ---- modules/gdnavigation/nav_region.cpp | 161 ----- modules/gdnavigation/nav_region.h | 101 --- modules/gdnavigation/nav_rid.h | 48 -- modules/gdnavigation/nav_utils.h | 140 ---- .../gdnavigation/navigation_mesh_editor_plugin.cpp | 154 ----- .../gdnavigation/navigation_mesh_editor_plugin.h | 88 --- modules/gdnavigation/navigation_mesh_generator.cpp | 583 ---------------- modules/gdnavigation/navigation_mesh_generator.h | 83 --- modules/gdnavigation/register_types.cpp | 82 --- modules/gdnavigation/register_types.h | 41 -- modules/gdnavigation/rvo_agent.cpp | 83 --- modules/gdnavigation/rvo_agent.h | 78 --- modules/navigation/SCsub | 63 ++ modules/navigation/config.py | 6 + modules/navigation/godot_navigation_server.cpp | 547 +++++++++++++++ modules/navigation/godot_navigation_server.h | 149 ++++ modules/navigation/nav_map.cpp | 764 +++++++++++++++++++++ modules/navigation/nav_map.h | 141 ++++ modules/navigation/nav_region.cpp | 161 +++++ modules/navigation/nav_region.h | 101 +++ modules/navigation/nav_rid.h | 48 ++ modules/navigation/nav_utils.h | 140 ++++ .../navigation/navigation_mesh_editor_plugin.cpp | 154 +++++ modules/navigation/navigation_mesh_editor_plugin.h | 88 +++ modules/navigation/navigation_mesh_generator.cpp | 583 ++++++++++++++++ modules/navigation/navigation_mesh_generator.h | 83 +++ modules/navigation/register_types.cpp | 74 ++ modules/navigation/register_types.h | 37 + modules/navigation/rvo_agent.cpp | 83 +++ modules/navigation/rvo_agent.h | 78 +++ 36 files changed, 3300 insertions(+), 3312 deletions(-) delete mode 100644 modules/gdnavigation/SCsub delete mode 100644 modules/gdnavigation/config.py delete mode 100644 modules/gdnavigation/gd_navigation_server.cpp delete mode 100644 modules/gdnavigation/gd_navigation_server.h delete mode 100644 modules/gdnavigation/nav_map.cpp delete mode 100644 modules/gdnavigation/nav_map.h delete mode 100644 modules/gdnavigation/nav_region.cpp delete mode 100644 modules/gdnavigation/nav_region.h delete mode 100644 modules/gdnavigation/nav_rid.h delete mode 100644 modules/gdnavigation/nav_utils.h delete mode 100644 modules/gdnavigation/navigation_mesh_editor_plugin.cpp delete mode 100644 modules/gdnavigation/navigation_mesh_editor_plugin.h delete mode 100644 modules/gdnavigation/navigation_mesh_generator.cpp delete mode 100644 modules/gdnavigation/navigation_mesh_generator.h delete mode 100644 modules/gdnavigation/register_types.cpp delete mode 100644 modules/gdnavigation/register_types.h delete mode 100644 modules/gdnavigation/rvo_agent.cpp delete mode 100644 modules/gdnavigation/rvo_agent.h create mode 100644 modules/navigation/SCsub create mode 100644 modules/navigation/config.py create mode 100644 modules/navigation/godot_navigation_server.cpp create mode 100644 modules/navigation/godot_navigation_server.h create mode 100644 modules/navigation/nav_map.cpp create mode 100644 modules/navigation/nav_map.h create mode 100644 modules/navigation/nav_region.cpp create mode 100644 modules/navigation/nav_region.h create mode 100644 modules/navigation/nav_rid.h create mode 100644 modules/navigation/nav_utils.h create mode 100644 modules/navigation/navigation_mesh_editor_plugin.cpp create mode 100644 modules/navigation/navigation_mesh_editor_plugin.h create mode 100644 modules/navigation/navigation_mesh_generator.cpp create mode 100644 modules/navigation/navigation_mesh_generator.h create mode 100644 modules/navigation/register_types.cpp create mode 100644 modules/navigation/register_types.h create mode 100644 modules/navigation/rvo_agent.cpp create mode 100644 modules/navigation/rvo_agent.h (limited to 'modules') diff --git a/modules/gdnavigation/SCsub b/modules/gdnavigation/SCsub deleted file mode 100644 index 22b5509b32..0000000000 --- a/modules/gdnavigation/SCsub +++ /dev/null @@ -1,63 +0,0 @@ -#!/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/gdnavigation/config.py b/modules/gdnavigation/config.py deleted file mode 100644 index d22f9454ed..0000000000 --- a/modules/gdnavigation/config.py +++ /dev/null @@ -1,6 +0,0 @@ -def can_build(env, platform): - return True - - -def configure(env): - pass diff --git a/modules/gdnavigation/gd_navigation_server.cpp b/modules/gdnavigation/gd_navigation_server.cpp deleted file mode 100644 index 929bbf9354..0000000000 --- a/modules/gdnavigation/gd_navigation_server.cpp +++ /dev/null @@ -1,547 +0,0 @@ -/*************************************************************************/ -/* gd_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 "gd_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(GdNavigationServer *server) { \ - server->MERGE(_cmd_, F_NAME)(d_0); \ - } \ - }; \ - void GdNavigationServer::F_NAME(T_0 D_0) const { \ - auto cmd = memnew(MERGE(F_NAME, _command)( \ - D_0)); \ - add_command(cmd); \ - } \ - void GdNavigationServer::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(GdNavigationServer *server) { \ - server->MERGE(_cmd_, F_NAME)(d_0, d_1); \ - } \ - }; \ - void GdNavigationServer::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 GdNavigationServer::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(GdNavigationServer *server) { \ - server->MERGE(_cmd_, F_NAME)(d_0, d_1, d_2, d_3); \ - } \ - }; \ - void GdNavigationServer::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 GdNavigationServer::MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3) - -GdNavigationServer::GdNavigationServer() : - NavigationServer3D() { -} - -GdNavigationServer::~GdNavigationServer() { - flush_queries(); -} - -void GdNavigationServer::add_command(SetCommand *command) const { - GdNavigationServer *mut_this = const_cast(this); - { - MutexLock lock(commands_mutex); - mut_this->commands.push_back(command); - } -} - -RID GdNavigationServer::map_create() const { - GdNavigationServer *mut_this = const_cast(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 GdNavigationServer::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 GdNavigationServer::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 GdNavigationServer::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 GdNavigationServer::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 GdNavigationServer::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()); - - return map->get_path(p_origin, p_destination, p_optimize, p_layers); -} - -Vector3 GdNavigationServer::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 GdNavigationServer::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 GdNavigationServer::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 GdNavigationServer::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 GdNavigationServer::region_create() const { - GdNavigationServer *mut_this = const_cast(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 GdNavigationServer::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, p_nav_mesh) { - NavRegion *region = region_owner.getornull(p_region); - ERR_FAIL_COND(region == nullptr); - - region->set_mesh(p_nav_mesh); -} - -void GdNavigationServer::region_bake_navmesh(Ref 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 GdNavigationServer::region_get_connections_count(RID p_region) const { - NavRegion *region = region_owner.getornull(p_region); - ERR_FAIL_COND_V(!region, 0); - - return region->get_connections_count(); -} - -Vector3 GdNavigationServer::region_get_connection_pathway_start(RID p_region, int p_connection_id) const { - NavRegion *region = region_owner.getornull(p_region); - ERR_FAIL_COND_V(!region, Vector3()); - - return region->get_connection_pathway_start(p_connection_id); -} - -Vector3 GdNavigationServer::region_get_connection_pathway_end(RID p_region, int p_connection_id) const { - NavRegion *region = region_owner.getornull(p_region); - ERR_FAIL_COND_V(!region, Vector3()); - - return region->get_connection_pathway_end(p_connection_id); -} - -RID GdNavigationServer::agent_create() const { - GdNavigationServer *mut_this = const_cast(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 GdNavigationServer::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 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 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 GdNavigationServer::set_active(bool p_active) const { - GdNavigationServer *mut_this = const_cast(this); - MutexLock lock(mut_this->operations_mutex); - mut_this->active = p_active; -} - -void GdNavigationServer::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 GdNavigationServer::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/gdnavigation/gd_navigation_server.h b/modules/gdnavigation/gd_navigation_server.h deleted file mode 100644 index bc2fc855c4..0000000000 --- a/modules/gdnavigation/gd_navigation_server.h +++ /dev/null @@ -1,149 +0,0 @@ -/*************************************************************************/ -/* gd_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 GD_NAVIGATION_SERVER_H -#define GD_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 GdNavigationServer; - -struct SetCommand { - virtual ~SetCommand() {} - virtual void exec(GdNavigationServer *server) = 0; -}; - -class GdNavigationServer : public NavigationServer3D { - Mutex commands_mutex; - /// Mutex used to make any operation threadsafe. - Mutex operations_mutex; - - std::vector commands; - - mutable RID_Owner map_owner; - mutable RID_Owner region_owner; - mutable RID_Owner agent_owner; - - bool active = true; - LocalVector active_maps; - LocalVector active_maps_update_id; - -public: - GdNavigationServer(); - virtual ~GdNavigationServer(); - - 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 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, p_nav_mesh); - virtual void region_bake_navmesh(Ref 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 // GD_NAVIGATION_SERVER_H diff --git a/modules/gdnavigation/nav_map.cpp b/modules/gdnavigation/nav_map.cpp deleted file mode 100644 index 41306f0687..0000000000 --- a/modules/gdnavigation/nav_map.cpp +++ /dev/null @@ -1,764 +0,0 @@ -/*************************************************************************/ -/* 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 - -/** - @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 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(); - } - if (begin_poly == end_poly) { - Vector path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - // List of all reachable navigation polys. - std::vector 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 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::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::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(); - } - - Vector 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::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::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::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> 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>::Element *connection = connections.find(ek); - if (!connection) { - connections[ek] = Vector(); - } - 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 free_edges; - for (Map>::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 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(controlled_agents.size()); i++) { - controlled_agents[i]->dispatch_callback(); - } -} - -void NavMap::clip_path(const std::vector &p_navigation_polys, Vector &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/gdnavigation/nav_map.h b/modules/gdnavigation/nav_map.h deleted file mode 100644 index 8e013a72eb..0000000000 --- a/modules/gdnavigation/nav_map.h +++ /dev/null @@ -1,141 +0,0 @@ -/*************************************************************************/ -/* 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 - -/** - @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 regions; - - /// Map polygons - std::vector polygons; - - /// Rvo world - RVO::KdTree rvo; - - /// Is agent array modified? - bool agents_dirty = false; - - /// All the Agents (even the controlled one) - std::vector agents; - - /// Controlled agents - std::vector 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 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 &get_regions() const { - return regions; - } - - bool has_agent(RvoAgent *agent) const; - void add_agent(RvoAgent *agent); - void remove_agent(RvoAgent *agent); - const std::vector &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 &p_navigation_polys, Vector &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/gdnavigation/nav_region.cpp b/modules/gdnavigation/nav_region.cpp deleted file mode 100644 index 81b15a49f5..0000000000 --- a/modules/gdnavigation/nav_region.cpp +++ /dev/null @@ -1,161 +0,0 @@ -/*************************************************************************/ -/* 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 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 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 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/gdnavigation/nav_region.h b/modules/gdnavigation/nav_region.h deleted file mode 100644 index f8b067e638..0000000000 --- a/modules/gdnavigation/nav_region.h +++ /dev/null @@ -1,101 +0,0 @@ -/*************************************************************************/ -/* 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 - -/** - @author AndreaCatania -*/ - -class NavMap; -class NavRegion; - -class NavRegion : public NavRid { - NavMap *map = nullptr; - Transform3D transform; - Ref mesh; - uint32_t layers = 1; - Vector connections; - - bool polygons_dirty = true; - - /// Cache - std::vector 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 p_mesh); - const Ref get_mesh() const { - return mesh; - } - - Vector &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 const &get_polygons() const { - return polygons; - } - - bool sync(); - -private: - void update_polygons(); -}; - -#endif // NAV_REGION_H diff --git a/modules/gdnavigation/nav_rid.h b/modules/gdnavigation/nav_rid.h deleted file mode 100644 index a0a60a3643..0000000000 --- a/modules/gdnavigation/nav_rid.h +++ /dev/null @@ -1,48 +0,0 @@ -/*************************************************************************/ -/* 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/gdnavigation/nav_utils.h b/modules/gdnavigation/nav_utils.h deleted file mode 100644 index 35da391eea..0000000000 --- a/modules/gdnavigation/nav_utils.h +++ /dev/null @@ -1,140 +0,0 @@ -/*************************************************************************/ -/* 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 - -/** - @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 connections; -}; - -struct Polygon { - NavRegion *owner; - - /// The points of this `Polygon` - std::vector points; - - /// Are the points clockwise ? - bool clockwise; - - /// The edges of this `Polygon` - std::vector 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/gdnavigation/navigation_mesh_editor_plugin.cpp b/modules/gdnavigation/navigation_mesh_editor_plugin.cpp deleted file mode 100644 index aa9248d2a1..0000000000 --- a/modules/gdnavigation/navigation_mesh_editor_plugin.cpp +++ /dev/null @@ -1,154 +0,0 @@ -/*************************************************************************/ -/* 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(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/gdnavigation/navigation_mesh_editor_plugin.h b/modules/gdnavigation/navigation_mesh_editor_plugin.h deleted file mode 100644 index c39269865b..0000000000 --- a/modules/gdnavigation/navigation_mesh_editor_plugin.h +++ /dev/null @@ -1,88 +0,0 @@ -/*************************************************************************/ -/* 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/gdnavigation/navigation_mesh_generator.cpp b/modules/gdnavigation/navigation_mesh_generator.cpp deleted file mode 100644 index 0d8330c1da..0000000000 --- a/modules/gdnavigation/navigation_mesh_generator.cpp +++ /dev/null @@ -1,583 +0,0 @@ -/*************************************************************************/ -/* 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 &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 &p_mesh, const Transform3D &p_xform, Vector &p_verticies, Vector &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 mesh_vertices = a[Mesh::ARRAY_VERTEX]; - const Vector3 *vr = mesh_vertices.ptr(); - - if (p_mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { - Vector 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 &p_verticies, Vector &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 &p_verticies, Vector &p_indices, int p_generate_from, uint32_t p_collision_mask, bool p_recurse_children) { - if (Object::cast_to(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) { - MeshInstance3D *mesh_instance = Object::cast_to(p_node); - Ref 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(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) { - CSGShape3D *csg_shape = Object::cast_to(p_node); - Array meshes = csg_shape->get_meshes(); - if (!meshes.is_empty()) { - Ref 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(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES) { - StaticBody3D *static_body = Object::cast_to(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(child)) { - CollisionShape3D *col_shape = Object::cast_to(child); - - Transform3D transform = p_accumulated_transform * static_body->get_transform() * col_shape->get_transform(); - - Ref mesh; - Ref s = col_shape->get_shape(); - - BoxShape3D *box = Object::cast_to(*s); - if (box) { - Ref box_mesh; - box_mesh.instantiate(); - box_mesh->set_size(box->get_size()); - mesh = box_mesh; - } - - CapsuleShape3D *capsule = Object::cast_to(*s); - if (capsule) { - Ref 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(*s); - if (cylinder) { - Ref 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(*s); - if (sphere) { - Ref 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(*s); - if (concave_polygon) { - _add_faces(concave_polygon->get_faces(), transform, p_verticies, p_indices); - } - - ConvexPolygonShape3D *convex_polygon = Object::cast_to(*s); - if (convex_polygon) { - Vector 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(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) { - GridMap *gridmap_instance = Object::cast_to(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 = 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(p_node)) { - Node3D *spatial = Object::cast_to(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 p_nav_mesh) { - Vector 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 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 p_nav_mesh, -#ifdef TOOLS_ENABLED - EditorProgress *ep, -#endif - rcHeightfield *hf, - rcCompactHeightfield *chf, - rcContourSet *cset, - rcPolyMesh *poly_mesh, - rcPolyMeshDetail *detail_mesh, - Vector &vertices, - Vector &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 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 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 vertices; - Vector indices; - - List 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(p_node)->get_transform().affine_inverse(); - for (const List::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 p_nav_mesh) { - if (p_nav_mesh.is_valid()) { - p_nav_mesh->clear_polygons(); - p_nav_mesh->set_vertices(Vector()); - } -} - -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/gdnavigation/navigation_mesh_generator.h b/modules/gdnavigation/navigation_mesh_generator.h deleted file mode 100644 index 847c7d097b..0000000000 --- a/modules/gdnavigation/navigation_mesh_generator.h +++ /dev/null @@ -1,83 +0,0 @@ -/*************************************************************************/ -/* 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 - -#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 &p_verticies); - static void _add_mesh(const Ref &p_mesh, const Transform3D &p_xform, Vector &p_verticies, Vector &p_indices); - static void _add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform, Vector &p_verticies, Vector &p_indices); - static void _parse_geometry(Transform3D p_accumulated_transform, Node *p_node, Vector &p_verticies, Vector &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 p_nav_mesh); - static void _build_recast_navigation_mesh( - Ref p_nav_mesh, -#ifdef TOOLS_ENABLED - EditorProgress *ep, -#endif - rcHeightfield *hf, - rcCompactHeightfield *chf, - rcContourSet *cset, - rcPolyMesh *poly_mesh, - rcPolyMeshDetail *detail_mesh, - Vector &vertices, - Vector &indices); - -public: - static NavigationMeshGenerator *get_singleton(); - - NavigationMeshGenerator(); - ~NavigationMeshGenerator(); - - void bake(Ref p_nav_mesh, Node *p_node); - void clear(Ref p_nav_mesh); -}; - -#endif - -#endif // NAVIGATION_MESH_GENERATOR_H diff --git a/modules/gdnavigation/register_types.cpp b/modules/gdnavigation/register_types.cpp deleted file mode 100644 index 8443d3d242..0000000000 --- a/modules/gdnavigation/register_types.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/*************************************************************************/ -/* 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 "gd_navigation_server.h" -#include "servers/navigation_server_3d.h" - -#ifndef _3D_DISABLED -#include "navigation_mesh_generator.h" -#endif - -#ifdef TOOLS_ENABLED -#include "navigation_mesh_editor_plugin.h" -#endif - -/** - @author AndreaCatania -*/ - -#ifndef _3D_DISABLED -NavigationMeshGenerator *_nav_mesh_generator = nullptr; -#endif - -NavigationServer3D *new_server() { - return memnew(GdNavigationServer); -} - -void register_gdnavigation_types() { - NavigationServer3DManager::set_default_server(new_server); - -#ifndef _3D_DISABLED - _nav_mesh_generator = memnew(NavigationMeshGenerator); - ClassDB::register_class(); - Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationMeshGenerator", NavigationMeshGenerator::get_singleton())); -#endif - -#ifdef TOOLS_ENABLED - EditorPlugins::add_by_type(); - - ClassDB::APIType prev_api = ClassDB::get_current_api(); - ClassDB::set_current_api(ClassDB::API_EDITOR); - - ClassDB::set_current_api(prev_api); -#endif -} - -void unregister_gdnavigation_types() { -#ifndef _3D_DISABLED - if (_nav_mesh_generator) { - memdelete(_nav_mesh_generator); - } -#endif -} diff --git a/modules/gdnavigation/register_types.h b/modules/gdnavigation/register_types.h deleted file mode 100644 index c2bb08c649..0000000000 --- a/modules/gdnavigation/register_types.h +++ /dev/null @@ -1,41 +0,0 @@ -/*************************************************************************/ -/* 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. */ -/*************************************************************************/ - -/** - @author AndreaCatania -*/ - -#ifndef GDNAVIGATION_REGISTER_TYPES_H -#define GDNAVIGATION_REGISTER_TYPES_H - -void register_gdnavigation_types(); -void unregister_gdnavigation_types(); - -#endif // GDNAVIGATION_REGISTER_TYPES_H diff --git a/modules/gdnavigation/rvo_agent.cpp b/modules/gdnavigation/rvo_agent.cpp deleted file mode 100644 index 21e43d08c1..0000000000 --- a/modules/gdnavigation/rvo_agent.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/*************************************************************************/ -/* 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/gdnavigation/rvo_agent.h b/modules/gdnavigation/rvo_agent.h deleted file mode 100644 index 369cb1f9a3..0000000000 --- a/modules/gdnavigation/rvo_agent.h +++ /dev/null @@ -1,78 +0,0 @@ -/*************************************************************************/ -/* 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 - -/** - @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 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(this); + { + MutexLock lock(commands_mutex); + mut_this->commands.push_back(command); + } +} + +RID GodotNavigationServer::map_create() const { + GodotNavigationServer *mut_this = const_cast(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 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()); + + 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(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, 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 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(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 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 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(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 commands; + + mutable RID_Owner map_owner; + mutable RID_Owner region_owner; + mutable RID_Owner agent_owner; + + bool active = true; + LocalVector active_maps; + LocalVector 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 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, p_nav_mesh); + virtual void region_bake_navmesh(Ref 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 + +/** + @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 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(); + } + if (begin_poly == end_poly) { + Vector path; + path.resize(2); + path.write[0] = begin_point; + path.write[1] = end_point; + return path; + } + + // List of all reachable navigation polys. + std::vector 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 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::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::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(); + } + + Vector 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::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::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::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> 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>::Element *connection = connections.find(ek); + if (!connection) { + connections[ek] = Vector(); + } + 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 free_edges; + for (Map>::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 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(controlled_agents.size()); i++) { + controlled_agents[i]->dispatch_callback(); + } +} + +void NavMap::clip_path(const std::vector &p_navigation_polys, Vector &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 + +/** + @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 regions; + + /// Map polygons + std::vector polygons; + + /// Rvo world + RVO::KdTree rvo; + + /// Is agent array modified? + bool agents_dirty = false; + + /// All the Agents (even the controlled one) + std::vector agents; + + /// Controlled agents + std::vector 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 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 &get_regions() const { + return regions; + } + + bool has_agent(RvoAgent *agent) const; + void add_agent(RvoAgent *agent); + void remove_agent(RvoAgent *agent); + const std::vector &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 &p_navigation_polys, Vector &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 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 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 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 + +/** + @author AndreaCatania +*/ + +class NavMap; +class NavRegion; + +class NavRegion : public NavRid { + NavMap *map = nullptr; + Transform3D transform; + Ref mesh; + uint32_t layers = 1; + Vector connections; + + bool polygons_dirty = true; + + /// Cache + std::vector 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 p_mesh); + const Ref get_mesh() const { + return mesh; + } + + Vector &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 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 + +/** + @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 connections; +}; + +struct Polygon { + NavRegion *owner; + + /// The points of this `Polygon` + std::vector points; + + /// Are the points clockwise ? + bool clockwise; + + /// The edges of this `Polygon` + std::vector 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(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 &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 &p_mesh, const Transform3D &p_xform, Vector &p_verticies, Vector &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 mesh_vertices = a[Mesh::ARRAY_VERTEX]; + const Vector3 *vr = mesh_vertices.ptr(); + + if (p_mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) { + Vector 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 &p_verticies, Vector &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 &p_verticies, Vector &p_indices, int p_generate_from, uint32_t p_collision_mask, bool p_recurse_children) { + if (Object::cast_to(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) { + MeshInstance3D *mesh_instance = Object::cast_to(p_node); + Ref 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(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) { + CSGShape3D *csg_shape = Object::cast_to(p_node); + Array meshes = csg_shape->get_meshes(); + if (!meshes.is_empty()) { + Ref 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(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES) { + StaticBody3D *static_body = Object::cast_to(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(child)) { + CollisionShape3D *col_shape = Object::cast_to(child); + + Transform3D transform = p_accumulated_transform * static_body->get_transform() * col_shape->get_transform(); + + Ref mesh; + Ref s = col_shape->get_shape(); + + BoxShape3D *box = Object::cast_to(*s); + if (box) { + Ref box_mesh; + box_mesh.instantiate(); + box_mesh->set_size(box->get_size()); + mesh = box_mesh; + } + + CapsuleShape3D *capsule = Object::cast_to(*s); + if (capsule) { + Ref 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(*s); + if (cylinder) { + Ref 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(*s); + if (sphere) { + Ref 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(*s); + if (concave_polygon) { + _add_faces(concave_polygon->get_faces(), transform, p_verticies, p_indices); + } + + ConvexPolygonShape3D *convex_polygon = Object::cast_to(*s); + if (convex_polygon) { + Vector 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(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) { + GridMap *gridmap_instance = Object::cast_to(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 = 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(p_node)) { + Node3D *spatial = Object::cast_to(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 p_nav_mesh) { + Vector 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 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 p_nav_mesh, +#ifdef TOOLS_ENABLED + EditorProgress *ep, +#endif + rcHeightfield *hf, + rcCompactHeightfield *chf, + rcContourSet *cset, + rcPolyMesh *poly_mesh, + rcPolyMeshDetail *detail_mesh, + Vector &vertices, + Vector &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 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 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 vertices; + Vector indices; + + List 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(p_node)->get_transform().affine_inverse(); + for (const List::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 p_nav_mesh) { + if (p_nav_mesh.is_valid()) { + p_nav_mesh->clear_polygons(); + p_nav_mesh->set_vertices(Vector()); + } +} + +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 + +#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 &p_verticies); + static void _add_mesh(const Ref &p_mesh, const Transform3D &p_xform, Vector &p_verticies, Vector &p_indices); + static void _add_faces(const PackedVector3Array &p_faces, const Transform3D &p_xform, Vector &p_verticies, Vector &p_indices); + static void _parse_geometry(Transform3D p_accumulated_transform, Node *p_node, Vector &p_verticies, Vector &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 p_nav_mesh); + static void _build_recast_navigation_mesh( + Ref p_nav_mesh, +#ifdef TOOLS_ENABLED + EditorProgress *ep, +#endif + rcHeightfield *hf, + rcCompactHeightfield *chf, + rcContourSet *cset, + rcPolyMesh *poly_mesh, + rcPolyMeshDetail *detail_mesh, + Vector &vertices, + Vector &indices); + +public: + static NavigationMeshGenerator *get_singleton(); + + NavigationMeshGenerator(); + ~NavigationMeshGenerator(); + + void bake(Ref p_nav_mesh, Node *p_node); + void clear(Ref 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(); + Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationMeshGenerator", NavigationMeshGenerator::get_singleton())); +#endif + +#ifdef TOOLS_ENABLED + EditorPlugins::add_by_type(); +#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 + +/** + @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 -- cgit v1.2.3