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