diff options
Diffstat (limited to 'modules/gdnavigation')
| -rw-r--r-- | modules/gdnavigation/SCsub | 50 | ||||
| -rw-r--r-- | modules/gdnavigation/config.py | 6 | ||||
| -rw-r--r-- | modules/gdnavigation/gd_navigation_server.cpp | 502 | ||||
| -rw-r--r-- | modules/gdnavigation/gd_navigation_server.h | 142 | ||||
| -rw-r--r-- | modules/gdnavigation/nav_map.cpp | 780 | ||||
| -rw-r--r-- | modules/gdnavigation/nav_map.h | 140 | ||||
| -rw-r--r-- | modules/gdnavigation/nav_region.cpp | 131 | ||||
| -rw-r--r-- | modules/gdnavigation/nav_region.h | 89 | ||||
| -rw-r--r-- | modules/gdnavigation/nav_rid.h | 48 | ||||
| -rw-r--r-- | modules/gdnavigation/nav_utils.h | 153 | ||||
| -rw-r--r-- | modules/gdnavigation/navigation_mesh_editor_plugin.cpp | 154 | ||||
| -rw-r--r-- | modules/gdnavigation/navigation_mesh_editor_plugin.h | 88 | ||||
| -rw-r--r-- | modules/gdnavigation/navigation_mesh_generator.cpp | 583 | ||||
| -rw-r--r-- | modules/gdnavigation/navigation_mesh_generator.h | 83 | ||||
| -rw-r--r-- | modules/gdnavigation/register_types.cpp | 82 | ||||
| -rw-r--r-- | modules/gdnavigation/register_types.h | 41 | ||||
| -rw-r--r-- | modules/gdnavigation/rvo_agent.cpp | 83 | ||||
| -rw-r--r-- | modules/gdnavigation/rvo_agent.h | 77 |
18 files changed, 3232 insertions, 0 deletions
diff --git a/modules/gdnavigation/SCsub b/modules/gdnavigation/SCsub new file mode 100644 index 0000000000..877d601c6a --- /dev/null +++ b/modules/gdnavigation/SCsub @@ -0,0 +1,50 @@ +#!/usr/bin/env python + +Import("env") +Import("env_modules") + +env_navigation = env_modules.Clone() + +# 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(env.modules_sources, thirdparty_sources) + + +# RVO Thirdparty source files +if env["builtin_rvo2"]: + thirdparty_dir = "#thirdparty/rvo2" + thirdparty_sources = [ + "/src/Agent.cpp", + "/src/KdTree.cpp", + ] + thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources] + + env_navigation.Prepend(CPPPATH=[thirdparty_dir + "/src"]) + + env_thirdparty = env_navigation.Clone() + env_thirdparty.disable_warnings() + env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources) + + +# Godot source files +env_navigation.add_source_files(env.modules_sources, "*.cpp") diff --git a/modules/gdnavigation/config.py b/modules/gdnavigation/config.py new file mode 100644 index 0000000000..d22f9454ed --- /dev/null +++ b/modules/gdnavigation/config.py @@ -0,0 +1,6 @@ +def can_build(env, platform): + return True + + +def configure(env): + pass diff --git a/modules/gdnavigation/gd_navigation_server.cpp b/modules/gdnavigation/gd_navigation_server.cpp new file mode 100644 index 0000000000..c80cdcfeab --- /dev/null +++ b/modules/gdnavigation/gd_navigation_server.cpp @@ -0,0 +1,502 @@ +/*************************************************************************/ +/* gd_navigation_server.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "gd_navigation_server.h" + +#include "core/os/mutex.h" + +#ifndef _3D_DISABLED +#include "navigation_mesh_generator.h" +#endif + +/** + @author AndreaCatania +*/ + +/// Creates a struct for each function and a function that once called creates +/// an instance of that struct with the submitted parameters. +/// Then, that struct is stored in an array; the `sync` function consume that array. + +#define COMMAND_1(F_NAME, T_0, D_0) \ + struct MERGE(F_NAME, _command) : public SetCommand { \ + T_0 d_0; \ + MERGE(F_NAME, _command) \ + (T_0 p_d_0) : \ + d_0(p_d_0) {} \ + virtual void exec(GdNavigationServer *server) { \ + server->MERGE(_cmd_, F_NAME)(d_0); \ + } \ + }; \ + void GdNavigationServer::F_NAME(T_0 D_0) const { \ + auto cmd = memnew(MERGE(F_NAME, _command)( \ + D_0)); \ + add_command(cmd); \ + } \ + void GdNavigationServer::MERGE(_cmd_, F_NAME)(T_0 D_0) + +#define COMMAND_2(F_NAME, T_0, D_0, T_1, D_1) \ + struct MERGE(F_NAME, _command) : public SetCommand { \ + T_0 d_0; \ + T_1 d_1; \ + MERGE(F_NAME, _command) \ + ( \ + T_0 p_d_0, \ + T_1 p_d_1) : \ + d_0(p_d_0), \ + d_1(p_d_1) {} \ + virtual void exec(GdNavigationServer *server) { \ + server->MERGE(_cmd_, F_NAME)(d_0, d_1); \ + } \ + }; \ + void GdNavigationServer::F_NAME(T_0 D_0, T_1 D_1) const { \ + auto cmd = memnew(MERGE(F_NAME, _command)( \ + D_0, \ + D_1)); \ + add_command(cmd); \ + } \ + void GdNavigationServer::MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1) + +#define COMMAND_4(F_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3) \ + struct MERGE(F_NAME, _command) : public SetCommand { \ + T_0 d_0; \ + T_1 d_1; \ + T_2 d_2; \ + T_3 d_3; \ + MERGE(F_NAME, _command) \ + ( \ + T_0 p_d_0, \ + T_1 p_d_1, \ + T_2 p_d_2, \ + T_3 p_d_3) : \ + d_0(p_d_0), \ + d_1(p_d_1), \ + d_2(p_d_2), \ + d_3(p_d_3) {} \ + virtual void exec(GdNavigationServer *server) { \ + server->MERGE(_cmd_, F_NAME)(d_0, d_1, d_2, d_3); \ + } \ + }; \ + void GdNavigationServer::F_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3) const { \ + auto cmd = memnew(MERGE(F_NAME, _command)( \ + D_0, \ + D_1, \ + D_2, \ + D_3)); \ + add_command(cmd); \ + } \ + void GdNavigationServer::MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3) + +GdNavigationServer::GdNavigationServer() : + NavigationServer3D() { +} + +GdNavigationServer::~GdNavigationServer() { + flush_queries(); +} + +void GdNavigationServer::add_command(SetCommand *command) const { + auto mut_this = const_cast<GdNavigationServer *>(this); + { + MutexLock lock(commands_mutex); + mut_this->commands.push_back(command); + } +} + +RID GdNavigationServer::map_create() const { + auto mut_this = const_cast<GdNavigationServer *>(this); + MutexLock lock(mut_this->operations_mutex); + NavMap *space = memnew(NavMap); + RID rid = map_owner.make_rid(space); + 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); + } + } else { + active_maps.erase(map); + } +} + +bool GdNavigationServer::map_is_active(RID p_map) const { + NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == nullptr, false); + + return active_maps.find(map) >= 0; +} + +COMMAND_2(map_set_up, RID, p_map, Vector3, p_up) { + NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND(map == nullptr); + + map->set_up(p_up); +} + +Vector3 GdNavigationServer::map_get_up(RID p_map) const { + const NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == nullptr, Vector3()); + + return map->get_up(); +} + +COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size) { + NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND(map == nullptr); + + map->set_cell_size(p_cell_size); +} + +real_t GdNavigationServer::map_get_cell_size(RID p_map) const { + const NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == nullptr, 0); + + return map->get_cell_size(); +} + +COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin) { + NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND(map == nullptr); + + map->set_edge_connection_margin(p_connection_margin); +} + +real_t GdNavigationServer::map_get_edge_connection_margin(RID p_map) const { + const NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == nullptr, 0); + + return map->get_edge_connection_margin(); +} + +Vector<Vector3> GdNavigationServer::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) 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); +} + +Vector3 GdNavigationServer::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { + const NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == nullptr, Vector3()); + + return map->get_closest_point_to_segment(p_from, p_to, p_use_collision); +} + +Vector3 GdNavigationServer::map_get_closest_point(RID p_map, const Vector3 &p_point) const { + const NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == nullptr, Vector3()); + + return map->get_closest_point(p_point); +} + +Vector3 GdNavigationServer::map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const { + const NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == nullptr, Vector3()); + + return map->get_closest_point_normal(p_point); +} + +RID GdNavigationServer::map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const { + const NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND_V(map == nullptr, RID()); + + return map->get_closest_point_owner(p_point); +} + +RID GdNavigationServer::region_create() const { + auto mut_this = const_cast<GdNavigationServer *>(this); + MutexLock lock(mut_this->operations_mutex); + NavRegion *reg = memnew(NavRegion); + RID rid = region_owner.make_rid(reg); + 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, Transform, p_transform) { + NavRegion *region = region_owner.getornull(p_region); + ERR_FAIL_COND(region == nullptr); + + region->set_transform(p_transform); +} + +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 GdNavigationServer::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 +} + +RID GdNavigationServer::agent_create() const { + auto mut_this = const_cast<GdNavigationServer *>(this); + MutexLock lock(mut_this->operations_mutex); + RvoAgent *agent = memnew(RvoAgent()); + RID rid = agent_owner.make_rid(agent); + agent->set_self(rid); + return rid; +} + +COMMAND_2(agent_set_map, RID, p_agent, RID, p_map) { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND(agent == nullptr); + + if (agent->get_map()) { + if (agent->get_map()->get_self() == p_map) { + return; // Pointless + } + + agent->get_map()->remove_agent(agent); + } + + agent->set_map(nullptr); + + if (p_map.is_valid()) { + NavMap *map = map_owner.getornull(p_map); + ERR_FAIL_COND(map == nullptr); + + agent->set_map(map); + map->add_agent(agent); + + if (agent->has_callback()) { + map->set_agent_as_controlled(agent); + } + } +} + +COMMAND_2(agent_set_neighbor_dist, RID, p_agent, real_t, p_dist) { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->get_agent()->neighborDist_ = p_dist; +} + +COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count) { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->get_agent()->maxNeighbors_ = p_count; +} + +COMMAND_2(agent_set_time_horizon, RID, p_agent, real_t, p_time) { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->get_agent()->timeHorizon_ = p_time; +} + +COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius) { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->get_agent()->radius_ = p_radius; +} + +COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed) { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->get_agent()->maxSpeed_ = p_max_speed; +} + +COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity) { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->get_agent()->velocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z); +} + +COMMAND_2(agent_set_target_velocity, RID, p_agent, Vector3, p_velocity) { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->get_agent()->prefVelocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z); +} + +COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position) { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->get_agent()->position_ = RVO::Vector3(p_position.x, p_position.y, p_position.z); +} + +COMMAND_2(agent_set_ignore_y, RID, p_agent, bool, p_ignore) { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->get_agent()->ignore_y_ = p_ignore; +} + +bool GdNavigationServer::agent_is_map_changed(RID p_agent) const { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND_V(agent == nullptr, false); + + return agent->is_map_changed(); +} + +COMMAND_4(agent_set_callback, RID, p_agent, Object *, p_receiver, StringName, p_method, Variant, p_udata) { + RvoAgent *agent = agent_owner.getornull(p_agent); + ERR_FAIL_COND(agent == nullptr); + + agent->set_callback(p_receiver == nullptr ? ObjectID() : p_receiver->get_instance_id(), p_method, p_udata); + + if (agent->get_map()) { + if (p_receiver == nullptr) { + agent->get_map()->remove_agent_as_controlled(agent); + } else { + agent->get_map()->set_agent_as_controlled(agent); + } + } +} + +COMMAND_1(free, RID, p_object) { + if (map_owner.owns(p_object)) { + NavMap *map = map_owner.getornull(p_object); + + // Removes any assigned region + std::vector<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); + } + + active_maps.erase(map); + map_owner.free(p_object); + memdelete(map); + + } 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); + memdelete(region); + + } 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); + memdelete(agent); + + } else { + ERR_FAIL_COND("Invalid ID."); + } +} + +void GdNavigationServer::set_active(bool p_active) const { + auto mut_this = const_cast<GdNavigationServer *>(this); + MutexLock lock(mut_this->operations_mutex); + mut_this->active = p_active; +} + +void GdNavigationServer::flush_queries() { + // In c++ we can't be sure that this is performed in the main thread + // even with mutable functions. + MutexLock lock(commands_mutex); + MutexLock lock2(operations_mutex); + for (size_t i(0); i < commands.size(); i++) { + commands[i]->exec(this); + memdelete(commands[i]); + } + commands.clear(); +} + +void GdNavigationServer::process(real_t p_delta_time) { + flush_queries(); + + if (!active) { + return; + } + + // In c++ we can't be sure that this is performed in the main thread + // even with mutable functions. + MutexLock lock(operations_mutex); + for (int i(0); i < active_maps.size(); i++) { + active_maps[i]->sync(); + active_maps[i]->step(p_delta_time); + active_maps[i]->dispatch_callbacks(); + } +} + +#undef COMMAND_1 +#undef COMMAND_2 +#undef COMMAND_4 diff --git a/modules/gdnavigation/gd_navigation_server.h b/modules/gdnavigation/gd_navigation_server.h new file mode 100644 index 0000000000..e3e02f3d7c --- /dev/null +++ b/modules/gdnavigation/gd_navigation_server.h @@ -0,0 +1,142 @@ +/*************************************************************************/ +/* gd_navigation_server.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef GD_NAVIGATION_SERVER_H +#define GD_NAVIGATION_SERVER_H + +#include "core/rid.h" +#include "core/rid_owner.h" +#include "servers/navigation_server_3d.h" + +#include "nav_map.h" +#include "nav_region.h" +#include "rvo_agent.h" + +/** + @author AndreaCatania +*/ + +/// The commands are functions executed during the `sync` phase. + +#define MERGE_INTERNAL(A, B) A##B +#define MERGE(A, B) MERGE_INTERNAL(A, B) + +#define COMMAND_1(F_NAME, T_0, D_0) \ + virtual void F_NAME(T_0 D_0) const; \ + void MERGE(_cmd_, F_NAME)(T_0 D_0) + +#define COMMAND_2(F_NAME, T_0, D_0, T_1, D_1) \ + virtual void F_NAME(T_0 D_0, T_1 D_1) const; \ + void MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1) + +#define COMMAND_4_DEF(F_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3, D_3_DEF) \ + virtual void F_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3 = D_3_DEF) const; \ + void MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3) + +class GdNavigationServer; + +struct SetCommand { + virtual ~SetCommand() {} + virtual void exec(GdNavigationServer *server) = 0; +}; + +class GdNavigationServer : public NavigationServer3D { + Mutex commands_mutex; + /// Mutex used to make any operation threadsafe. + Mutex operations_mutex; + + std::vector<SetCommand *> commands; + + mutable RID_PtrOwner<NavMap> map_owner; + mutable RID_PtrOwner<NavRegion> region_owner; + mutable RID_PtrOwner<RvoAgent> agent_owner; + + bool active = true; + Vector<NavMap *> active_maps; + +public: + GdNavigationServer(); + virtual ~GdNavigationServer(); + + void add_command(SetCommand *command) const; + + virtual RID map_create() const; + COMMAND_2(map_set_active, RID, p_map, bool, p_active); + virtual bool map_is_active(RID p_map) const; + + COMMAND_2(map_set_up, RID, p_map, Vector3, p_up); + virtual Vector3 map_get_up(RID p_map) const; + + COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size); + virtual real_t map_get_cell_size(RID p_map) const; + + COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin); + virtual real_t map_get_edge_connection_margin(RID p_map) const; + + virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) 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_transform, RID, p_region, Transform, 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 RID agent_create() const; + COMMAND_2(agent_set_map, RID, p_agent, RID, p_map); + COMMAND_2(agent_set_neighbor_dist, RID, p_agent, real_t, p_dist); + COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count); + COMMAND_2(agent_set_time_horizon, RID, p_agent, real_t, p_time); + COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius); + COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed); + COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity); + COMMAND_2(agent_set_target_velocity, RID, p_agent, Vector3, p_velocity); + COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position); + COMMAND_2(agent_set_ignore_y, RID, p_agent, bool, p_ignore); + virtual bool agent_is_map_changed(RID p_agent) const; + COMMAND_4_DEF(agent_set_callback, RID, p_agent, Object *, p_receiver, StringName, p_method, Variant, p_udata, Variant()); + + COMMAND_1(free, RID, p_object); + + virtual void set_active(bool p_active) const; + + void flush_queries(); + virtual void process(real_t p_delta_time); +}; + +#undef COMMAND_1 +#undef COMMAND_2 +#undef COMMAND_4_DEF + +#endif // GD_NAVIGATION_SERVER_H diff --git a/modules/gdnavigation/nav_map.cpp b/modules/gdnavigation/nav_map.cpp new file mode 100644 index 0000000000..7919e6a01f --- /dev/null +++ b/modules/gdnavigation/nav_map.cpp @@ -0,0 +1,780 @@ +/*************************************************************************/ +/* nav_map.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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 USE_ENTRY_POINT + +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) const { + 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]; + + // For each point cast a face and check the distance between the origin/destination + for (size_t point_id = 2; point_id < p.points.size(); point_id++) { + Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos); + Vector3 spoint = f.get_closest_point_to(p_origin); + float dpoint = spoint.distance_to(p_origin); + if (dpoint < begin_d) { + begin_d = dpoint; + begin_poly = &p; + begin_point = spoint; + } + + spoint = f.get_closest_point_to(p_destination); + dpoint = spoint.distance_to(p_destination); + if (dpoint < end_d) { + end_d = dpoint; + end_poly = &p; + end_point = spoint; + } + } + } + + if (!begin_poly || !end_poly) { + // No path + 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; + } + + std::vector<gd::NavigationPoly> navigation_polys; + navigation_polys.reserve(polygons.size() * 0.75); + + // The elements indices in the `navigation_polys`. + int least_cost_id(-1); + List<uint32_t> open_list; + bool found_route = false; + + navigation_polys.push_back(gd::NavigationPoly(begin_poly)); + { + least_cost_id = 0; + gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id]; + least_cost_poly->self_id = least_cost_id; + least_cost_poly->entry = begin_point; + } + + open_list.push_back(0); + + const gd::Polygon *reachable_end = nullptr; + float reachable_d = 1e30; + bool is_reachable = true; + + while (found_route == false) { + { + // Takes the current least_cost_poly neighbors and compute the traveled_distance of each + for (size_t i = 0; i < navigation_polys[least_cost_id].poly->edges.size(); i++) { + gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id]; + + const gd::Edge &edge = least_cost_poly->poly->edges[i]; + if (!edge.other_polygon) { + continue; + } + +#ifdef USE_ENTRY_POINT + Vector3 edge_line[2] = { + least_cost_poly->poly->points[i].pos, + least_cost_poly->poly->points[(i + 1) % least_cost_poly->poly->points.size()].pos + }; + + const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly->entry, edge_line); + const float new_distance = least_cost_poly->entry.distance_to(new_entry) + least_cost_poly->traveled_distance; +#else + const float new_distance = least_cost_poly->poly->center.distance_to(edge.other_polygon->center) + least_cost_poly->traveled_distance; +#endif + + auto it = std::find( + navigation_polys.begin(), + navigation_polys.end(), + gd::NavigationPoly(edge.other_polygon)); + + if (it != navigation_polys.end()) { + // Oh this was visited already, can we win the cost? + if (it->traveled_distance > new_distance) { + it->prev_navigation_poly_id = least_cost_id; + it->back_navigation_edge = edge.other_edge; + it->traveled_distance = new_distance; +#ifdef USE_ENTRY_POINT + it->entry = new_entry; +#endif + } + } else { + // Add to open neighbours + + navigation_polys.push_back(gd::NavigationPoly(edge.other_polygon)); + gd::NavigationPoly *np = &navigation_polys[navigation_polys.size() - 1]; + + np->self_id = navigation_polys.size() - 1; + np->prev_navigation_poly_id = least_cost_id; + np->back_navigation_edge = edge.other_edge; + np->traveled_distance = new_distance; +#ifdef USE_ENTRY_POINT + np->entry = new_entry; +#endif + open_list.push_back(navigation_polys.size() - 1); + } + } + } + + // Removes the least cost polygon from the open list so we can advance. + open_list.erase(least_cost_id); + + if (open_list.size() == 0) { + // When the open list is empty at this point the End Polygon is not reachable + // so 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); + open_list.clear(); + open_list.push_back(0); + + reachable_end = nullptr; + + continue; + } + + // Now take the new least_cost_poly from the open list. + least_cost_id = -1; + float least_cost = 1e30; + + for (auto element = open_list.front(); element != nullptr; element = element->next()) { + gd::NavigationPoly *np = &navigation_polys[element->get()]; + float cost = np->traveled_distance; +#ifdef USE_ENTRY_POINT + cost += np->entry.distance_to(end_point); +#else + cost += np->poly->center.distance_to(end_point); +#endif + 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) { + // Yep, done!! + found_route = true; + break; + } + } + + if (found_route) { + Vector<Vector3> path; + if (p_optimize) { + // String pulling + + gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id]; + Vector3 apex_point = end_point; + Vector3 portal_left = apex_point; + Vector3 portal_right = apex_point; + gd::NavigationPoly *left_poly = apex_poly; + gd::NavigationPoly *right_poly = apex_poly; + gd::NavigationPoly *p = apex_poly; + + path.push_back(end_point); + + while (p) { + Vector3 left; + Vector3 right; + +#define CLOCK_TANGENT(m_a, m_b, m_c) (((m_a) - (m_c)).cross((m_a) - (m_b))) + + if (p->poly == begin_poly) { + left = begin_point; + right = begin_point; + } else { + int prev = p->back_navigation_edge; + int prev_n = (p->back_navigation_edge + 1) % p->poly->points.size(); + left = p->poly->points[prev].pos; + right = p->poly->points[prev_n].pos; + + if (p->poly->clockwise) { + SWAP(left, right); + } + } + + bool skip = false; + + if (CLOCK_TANGENT(apex_point, portal_left, left).dot(up) >= 0) { + //process + if (portal_left == apex_point || CLOCK_TANGENT(apex_point, left, portal_right).dot(up) > 0) { + left_poly = p; + portal_left = left; + } else { + clip_path(navigation_polys, path, apex_poly, portal_right, right_poly); + + apex_point = portal_right; + p = right_poly; + left_poly = p; + apex_poly = p; + portal_left = apex_point; + portal_right = apex_point; + path.push_back(apex_point); + skip = true; + } + } + + if (!skip && CLOCK_TANGENT(apex_point, portal_right, right).dot(up) <= 0) { + //process + if (portal_right == apex_point || CLOCK_TANGENT(apex_point, right, portal_left).dot(up) < 0) { + right_poly = p; + portal_right = right; + } else { + clip_path(navigation_polys, path, apex_poly, portal_left, left_poly); + + apex_point = portal_left; + p = left_poly; + right_poly = p; + apex_poly = p; + portal_right = apex_point; + portal_left = apex_point; + path.push_back(apex_point); + } + } + + if (p->prev_navigation_poly_id != -1) { + p = &navigation_polys[p->prev_navigation_poly_id]; + } else { + // The end + p = nullptr; + } + } + + if (path[path.size() - 1] != begin_point) { + path.push_back(begin_point); + } + + path.invert(); + + } else { + path.push_back(end_point); + + // Add mid points + int np_id = least_cost_id; + while (np_id != -1) { +#ifdef USE_ENTRY_POINT + Vector3 point = navigation_polys[np_id].entry; +#else + int prev = navigation_polys[np_id].back_navigation_edge; + int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size(); + Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5; +#endif + + path.push_back(point); + np_id = navigation_polys[np_id].prev_navigation_poly_id; + } + + path.invert(); + } + + return path; + } + return Vector<Vector3>(); +} + +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) { + 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); + auto 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) { + auto it = std::find(controlled_agents.begin(), controlled_agents.end(), agent); + if (it != controlled_agents.end()) { + controlled_agents.erase(it); + } +} + +void NavMap::sync() { + 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) { + // Copy all region polygons in the map. + int count = 0; + for (size_t r(0); r < regions.size(); r++) { + count += regions[r]->get_polygons().size(); + } + + polygons.resize(count); + 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(); + } + + // Connects the `Edges` of all the `Polygons` of all `Regions` each other. + Map<gd::EdgeKey, gd::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, gd::Connection>::Element *connection = connections.find(ek); + if (!connection) { + // Nothing yet + gd::Connection c; + c.A = &poly; + c.A_edge = p; + c.B = nullptr; + c.B_edge = -1; + connections[ek] = c; + + } else if (connection->get().B == nullptr) { + CRASH_COND(connection->get().A == nullptr); // Unreachable + + // Connect the two Polygons by this edge + connection->get().B = &poly; + connection->get().B_edge = p; + + connection->get().A->edges[connection->get().A_edge].this_edge = connection->get().A_edge; + connection->get().A->edges[connection->get().A_edge].other_polygon = connection->get().B; + connection->get().A->edges[connection->get().A_edge].other_edge = connection->get().B_edge; + + connection->get().B->edges[connection->get().B_edge].this_edge = connection->get().B_edge; + connection->get().B->edges[connection->get().B_edge].other_polygon = connection->get().A; + connection->get().B->edges[connection->get().B_edge].other_edge = connection->get().A_edge; + } 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 Navigation3D's `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem."); + } + } + } + + // Takes all the free edges. + std::vector<gd::FreeEdge> free_edges; + free_edges.reserve(connections.size()); + + for (auto connection_element = connections.front(); connection_element; connection_element = connection_element->next()) { + if (connection_element->get().B == nullptr) { + CRASH_COND(connection_element->get().A == nullptr); // Unreachable + CRASH_COND(connection_element->get().A_edge < 0); // Unreachable + + // This is a free edge + uint32_t id(free_edges.size()); + free_edges.push_back(gd::FreeEdge()); + free_edges[id].is_free = true; + free_edges[id].poly = connection_element->get().A; + free_edges[id].edge_id = connection_element->get().A_edge; + uint32_t point_0(free_edges[id].edge_id); + uint32_t point_1((free_edges[id].edge_id + 1) % free_edges[id].poly->points.size()); + Vector3 pos_0 = free_edges[id].poly->points[point_0].pos; + Vector3 pos_1 = free_edges[id].poly->points[point_1].pos; + Vector3 relative = pos_1 - pos_0; + free_edges[id].edge_center = (pos_0 + pos_1) / 2.0; + free_edges[id].edge_dir = relative.normalized(); + free_edges[id].edge_len_squared = relative.length_squared(); + } + } + + const float ecm_squared(edge_connection_margin * edge_connection_margin); +#define LEN_TOLLERANCE 0.1 +#define DIR_TOLLERANCE 0.9 + // In front of tolerance +#define IFO_TOLLERANCE 0.5 + + // 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 (size_t i(0); i < free_edges.size(); i++) { + if (!free_edges[i].is_free) { + continue; + } + gd::FreeEdge &edge = free_edges[i]; + for (size_t y(0); y < free_edges.size(); y++) { + gd::FreeEdge &other_edge = free_edges[y]; + if (i == y || !other_edge.is_free || edge.poly->owner == other_edge.poly->owner) { + continue; + } + + Vector3 rel_centers = other_edge.edge_center - edge.edge_center; + if (ecm_squared > rel_centers.length_squared() // Are enough closer? + && ABS(edge.edge_len_squared - other_edge.edge_len_squared) < LEN_TOLLERANCE // Are the same length? + && ABS(edge.edge_dir.dot(other_edge.edge_dir)) > DIR_TOLLERANCE // Are aligned? + && ABS(rel_centers.normalized().dot(edge.edge_dir)) < IFO_TOLLERANCE // Are one in front the other? + ) { + // The edges can be connected + edge.is_free = false; + other_edge.is_free = false; + + edge.poly->edges[edge.edge_id].this_edge = edge.edge_id; + edge.poly->edges[edge.edge_id].other_edge = other_edge.edge_id; + edge.poly->edges[edge.edge_id].other_polygon = other_edge.poly; + + other_edge.poly->edges[other_edge.edge_id].this_edge = other_edge.edge_id; + other_edge.poly->edges[other_edge.edge_id].other_edge = edge.edge_id; + other_edge.poly->edges[other_edge.edge_id].other_polygon = edge.poly; + } + } + } + } + + if (regenerate_links) { + map_update_id = map_update_id + 1 % 9999999; + } + + if (agents_dirty) { + std::vector<RVO::Agent *> raw_agents; + raw_agents.reserve(agents.size()); + for (size_t i(0); i < agents.size(); i++) { + raw_agents.push_back(agents[i]->get_agent()); + } + rvo.buildAgentTree(raw_agents); + } + + regenerate_polygons = false; + regenerate_links = false; + agents_dirty = false; +} + +void NavMap::compute_single_step(uint32_t index, RvoAgent **agent) { + (*(agent + index))->get_agent()->computeNeighbors(&rvo); + (*(agent + index))->get_agent()->computeNewVelocity(deltatime); +} + +void NavMap::step(real_t p_deltatime) { + deltatime = p_deltatime; + if (controlled_agents.size() > 0) { + thread_process_array( + controlled_agents.size(), + this, + &NavMap::compute_single_step, + controlled_agents.data()); + } +} + +void NavMap::dispatch_callbacks() { + for (int i(0); i < static_cast<int>(controlled_agents.size()); i++) { + controlled_agents[i]->dispatch_callback(); + } +} + +void NavMap::clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const { + Vector3 from = path[path.size() - 1]; + + if (from.distance_to(p_to_point) < CMP_EPSILON) { + return; + } + Plane cut_plane; + cut_plane.normal = (from - p_to_point).cross(up); + if (cut_plane.normal == Vector3()) { + return; + } + cut_plane.normal.normalize(); + cut_plane.d = cut_plane.normal.dot(from); + + while (from_poly != p_to_poly) { + int back_nav_edge = from_poly->back_navigation_edge; + Vector3 a = from_poly->poly->points[back_nav_edge].pos; + Vector3 b = from_poly->poly->points[(back_nav_edge + 1) % from_poly->poly->points.size()].pos; + + ERR_FAIL_COND(from_poly->prev_navigation_poly_id == -1); + from_poly = &p_navigation_polys[from_poly->prev_navigation_poly_id]; + + if (a.distance_to(b) > CMP_EPSILON) { + Vector3 inters; + if (cut_plane.intersects_segment(a, b, &inters)) { + if (inters.distance_to(p_to_point) > CMP_EPSILON && inters.distance_to(path[path.size() - 1]) > CMP_EPSILON) { + path.push_back(inters); + } + } + } + } +} diff --git a/modules/gdnavigation/nav_map.h b/modules/gdnavigation/nav_map.h new file mode 100644 index 0000000000..892755f3f9 --- /dev/null +++ b/modules/gdnavigation/nav_map.h @@ -0,0 +1,140 @@ +/*************************************************************************/ +/* nav_map.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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 "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) 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/gdnavigation/nav_region.cpp b/modules/gdnavigation/nav_region.cpp new file mode 100644 index 0000000000..51fba67cc3 --- /dev/null +++ b/modules/gdnavigation/nav_region.cpp @@ -0,0 +1,131 @@ +/*************************************************************************/ +/* nav_region.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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; +} + +void NavRegion::set_transform(Transform p_transform) { + transform = p_transform; + polygons_dirty = true; +} + +void NavRegion::set_mesh(Ref<NavigationMesh> p_mesh) { + mesh = p_mesh; + polygons_dirty = true; +} + +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/gdnavigation/nav_region.h b/modules/gdnavigation/nav_region.h new file mode 100644 index 0000000000..731855bfb5 --- /dev/null +++ b/modules/gdnavigation/nav_region.h @@ -0,0 +1,89 @@ +/*************************************************************************/ +/* nav_region.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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 "nav_rid.h" + +#include "nav_utils.h" +#include "scene/3d/navigation_3d.h" +#include <vector> + +/** + @author AndreaCatania +*/ + +class NavMap; +class NavRegion; + +class NavRegion : public NavRid { + NavMap *map = nullptr; + Transform transform; + Ref<NavigationMesh> mesh; + + 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_transform(Transform transform); + const Transform &get_transform() const { + return transform; + } + + void set_mesh(Ref<NavigationMesh> p_mesh); + const Ref<NavigationMesh> get_mesh() const { + return mesh; + } + + std::vector<gd::Polygon> const &get_polygons() const { + return polygons; + } + + bool sync(); + +private: + void update_polygons(); +}; + +#endif // NAV_REGION_H diff --git a/modules/gdnavigation/nav_rid.h b/modules/gdnavigation/nav_rid.h new file mode 100644 index 0000000000..c119ecc5e0 --- /dev/null +++ b/modules/gdnavigation/nav_rid.h @@ -0,0 +1,48 @@ +/*************************************************************************/ +/* nav_rid.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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/rid.h" + +/** + @author AndreaCatania +*/ + +class NavRid { + RID self; + +public: + _FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } +}; + +#endif // NAV_RID_H diff --git a/modules/gdnavigation/nav_utils.h b/modules/gdnavigation/nav_utils.h new file mode 100644 index 0000000000..40e54df553 --- /dev/null +++ b/modules/gdnavigation/nav_utils.h @@ -0,0 +1,153 @@ +/*************************************************************************/ +/* nav_utils.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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; + 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; + + /// Other Polygon + Polygon *other_polygon = nullptr; + + /// The other `Polygon` at this edge id has this `Polygon`. + int other_edge = -1; + + Edge() {} +}; + +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 Connection { + Polygon *A = nullptr; + int A_edge = -1; + Polygon *B = nullptr; + int B_edge = -1; + + Connection() {} +}; + +struct NavigationPoly { + uint32_t self_id = 0; + /// This poly. + const Polygon *poly; + /// The previous navigation poly (id in the `navigation_poly` array). + int prev_navigation_poly_id = -1; + /// The edge id in this `Poly` to reach the `prev_navigation_poly_id`. + uint32_t back_navigation_edge = 0; + /// 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); + } +}; + +struct FreeEdge { + bool is_free; + Polygon *poly; + uint32_t edge_id; + Vector3 edge_center; + Vector3 edge_dir; + float edge_len_squared; +}; +} // namespace gd + +#endif // NAV_UTILS_H diff --git a/modules/gdnavigation/navigation_mesh_editor_plugin.cpp b/modules/gdnavigation/navigation_mesh_editor_plugin.cpp new file mode 100644 index 0000000000..648f4f7cdd --- /dev/null +++ b/modules/gdnavigation/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-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifdef TOOLS_ENABLED +#include "navigation_mesh_editor_plugin.h" + +#include "core/io/marshalls.h" +#include "core/io/resource_saver.h" +#include "navigation_mesh_generator.h" +#include "scene/3d/mesh_instance_3d.h" +#include "scene/gui/box_container.h" + +void NavigationMeshEditor::_node_removed(Node *p_node) { + if (p_node == node) { + node = nullptr; + + hide(); + } +} + +void NavigationMeshEditor::_notification(int p_option) { + if (p_option == NOTIFICATION_ENTER_TREE) { + button_bake->set_icon(get_theme_icon("Bake", "EditorIcons")); + button_reset->set_icon(get_theme_icon("Reload", "EditorIcons")); + } +} + +void NavigationMeshEditor::_bake_pressed() { + button_bake->set_pressed(false); + + ERR_FAIL_COND(!node); + if (!node->get_navigation_mesh().is_valid()) { + err_dialog->set_text(TTR("A NavigationMesh resource must be set or created for this node to work.")); + err_dialog->popup_centered(); + return; + } + + NavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh()); + NavigationMeshGenerator::get_singleton()->bake(node->get_navigation_mesh(), node); + + node->update_gizmo(); +} + +void NavigationMeshEditor::_clear_pressed() { + if (node) { + NavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh()); + } + + button_bake->set_pressed(false); + bake_info->set_text(""); + + if (node) { + node->update_gizmo(); + } +} + +void NavigationMeshEditor::edit(NavigationRegion3D *p_nav_region) { + if (p_nav_region == nullptr || node == p_nav_region) { + return; + } + + node = p_nav_region; +} + +void NavigationMeshEditor::_bind_methods() { +} + +NavigationMeshEditor::NavigationMeshEditor() { + bake_hbox = memnew(HBoxContainer); + + button_bake = memnew(Button); + button_bake->set_flat(true); + bake_hbox->add_child(button_bake); + button_bake->set_toggle_mode(true); + button_bake->set_text(TTR("Bake NavMesh")); + button_bake->connect("pressed", callable_mp(this, &NavigationMeshEditor::_bake_pressed)); + + button_reset = memnew(Button); + button_reset->set_flat(true); + bake_hbox->add_child(button_reset); + // No button text, we only use a revert icon which is set when entering the tree. + button_reset->set_tooltip(TTR("Clear the navigation mesh.")); + button_reset->connect("pressed", callable_mp(this, &NavigationMeshEditor::_clear_pressed)); + + bake_info = memnew(Label); + bake_hbox->add_child(bake_info); + + err_dialog = memnew(AcceptDialog); + add_child(err_dialog); + node = nullptr; +} + +NavigationMeshEditor::~NavigationMeshEditor() { +} + +void NavigationMeshEditorPlugin::edit(Object *p_object) { + navigation_mesh_editor->edit(Object::cast_to<NavigationRegion3D>(p_object)); +} + +bool NavigationMeshEditorPlugin::handles(Object *p_object) const { + return p_object->is_class("NavigationRegion3D"); +} + +void NavigationMeshEditorPlugin::make_visible(bool p_visible) { + if (p_visible) { + navigation_mesh_editor->show(); + navigation_mesh_editor->bake_hbox->show(); + } else { + navigation_mesh_editor->hide(); + navigation_mesh_editor->bake_hbox->hide(); + navigation_mesh_editor->edit(nullptr); + } +} + +NavigationMeshEditorPlugin::NavigationMeshEditorPlugin(EditorNode *p_node) { + editor = p_node; + navigation_mesh_editor = memnew(NavigationMeshEditor); + editor->get_viewport()->add_child(navigation_mesh_editor); + add_control_to_container(CONTAINER_SPATIAL_EDITOR_MENU, navigation_mesh_editor->bake_hbox); + navigation_mesh_editor->hide(); + navigation_mesh_editor->bake_hbox->hide(); +} + +NavigationMeshEditorPlugin::~NavigationMeshEditorPlugin() { +} + +#endif diff --git a/modules/gdnavigation/navigation_mesh_editor_plugin.h b/modules/gdnavigation/navigation_mesh_editor_plugin.h new file mode 100644 index 0000000000..f09182fff4 --- /dev/null +++ b/modules/gdnavigation/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-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#ifndef NAVIGATION_MESH_EDITOR_PLUGIN_H +#define NAVIGATION_MESH_EDITOR_PLUGIN_H + +#ifdef TOOLS_ENABLED + +#include "editor/editor_node.h" +#include "editor/editor_plugin.h" + +class NavigationRegion3D; + +class NavigationMeshEditor : public Control { + friend class NavigationMeshEditorPlugin; + + GDCLASS(NavigationMeshEditor, Control); + + AcceptDialog *err_dialog; + + HBoxContainer *bake_hbox; + Button *button_bake; + Button *button_reset; + Label *bake_info; + + NavigationRegion3D *node; + + void _bake_pressed(); + void _clear_pressed(); + +protected: + void _node_removed(Node *p_node); + static void _bind_methods(); + void _notification(int p_option); + +public: + void edit(NavigationRegion3D *p_nav_region); + NavigationMeshEditor(); + ~NavigationMeshEditor(); +}; + +class NavigationMeshEditorPlugin : public EditorPlugin { + GDCLASS(NavigationMeshEditorPlugin, EditorPlugin); + + NavigationMeshEditor *navigation_mesh_editor; + EditorNode *editor; + +public: + virtual String get_name() const override { return "NavigationMesh"; } + bool has_main_screen() const override { return false; } + virtual void edit(Object *p_object) override; + virtual bool handles(Object *p_object) const override; + virtual void make_visible(bool p_visible) override; + + NavigationMeshEditorPlugin(EditorNode *p_node); + ~NavigationMeshEditorPlugin(); +}; + +#endif + +#endif diff --git a/modules/gdnavigation/navigation_mesh_generator.cpp b/modules/gdnavigation/navigation_mesh_generator.cpp new file mode 100644 index 0000000000..5329600e39 --- /dev/null +++ b/modules/gdnavigation/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-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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/quick_hull.h" +#include "core/os/thread.h" +#include "scene/3d/collision_shape_3d.h" +#include "scene/3d/mesh_instance_3d.h" +#include "scene/3d/physics_body_3d.h" +#include "scene/resources/box_shape_3d.h" +#include "scene/resources/capsule_shape_3d.h" +#include "scene/resources/concave_polygon_shape_3d.h" +#include "scene/resources/convex_polygon_shape_3d.h" +#include "scene/resources/cylinder_shape_3d.h" +#include "scene/resources/primitive_meshes.h" +#include "scene/resources/shape_3d.h" +#include "scene/resources/sphere_shape_3d.h" +#include "scene/resources/world_margin_shape_3d.h" + +#include "modules/modules_enabled.gen.h" +#ifdef TOOLS_ENABLED +#include "editor/editor_node.h" +#include "editor/editor_settings.h" +#endif + +#ifdef MODULE_CSG_ENABLED +#include "modules/csg/csg_shape.h" +#endif +#ifdef MODULE_GRIDMAP_ENABLED +#include "modules/gridmap/grid_map.h" +#endif + +NavigationMeshGenerator *NavigationMeshGenerator::singleton = nullptr; + +void NavigationMeshGenerator::_add_vertex(const Vector3 &p_vec3, Vector<float> &p_verticies) { + p_verticies.push_back(p_vec3.x); + p_verticies.push_back(p_vec3.y); + p_verticies.push_back(p_vec3.z); +} + +void NavigationMeshGenerator::_add_mesh(const Ref<Mesh> &p_mesh, const Transform &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 Transform &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(Transform p_accumulated_transform, Node *p_node, Vector<float> &p_verticies, Vector<int> &p_indices, int p_generate_from, uint32_t p_collision_mask, bool p_recurse_children) { + if (Object::cast_to<MeshInstance3D>(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) { + MeshInstance3D *mesh_instance = Object::cast_to<MeshInstance3D>(p_node); + Ref<Mesh> mesh = mesh_instance->get_mesh(); + if (mesh.is_valid()) { + _add_mesh(mesh, p_accumulated_transform * mesh_instance->get_transform(), p_verticies, p_indices); + } + } + +#ifdef MODULE_CSG_ENABLED + if (Object::cast_to<CSGShape3D>(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) { + CSGShape3D *csg_shape = Object::cast_to<CSGShape3D>(p_node); + Array meshes = csg_shape->get_meshes(); + if (!meshes.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); + + Transform 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<CubeMesh> cube_mesh; + cube_mesh.instance(); + cube_mesh->set_size(box->get_extents() * 2.0); + mesh = cube_mesh; + } + + CapsuleShape3D *capsule = Object::cast_to<CapsuleShape3D>(*s); + if (capsule) { + Ref<CapsuleMesh> capsule_mesh; + capsule_mesh.instance(); + 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.instance(); + 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.instance(); + 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 = QuickHull::build(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(); + Transform 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 * 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); + } + + Transform navmesh_xform = Object::cast_to<Node3D>(p_node)->get_transform().affine_inverse(); + for (const List<Node *>::Element *E = parse_nodes.front(); E; E = E->next()) { + int geometry_type = p_nav_mesh->get_parsed_geometry_type(); + uint32_t collision_mask = p_nav_mesh->get_collision_mask(); + bool recurse_children = p_nav_mesh->get_source_geometry_mode() != NavigationMesh::SOURCE_GEOMETRY_GROUPS_EXPLICIT; + _parse_geometry(navmesh_xform, E->get(), vertices, indices, geometry_type, collision_mask, recurse_children); + } + + if (vertices.size() > 0 && indices.size() > 0) { + rcHeightfield *hf = nullptr; + rcCompactHeightfield *chf = nullptr; + rcContourSet *cset = nullptr; + rcPolyMesh *poly_mesh = nullptr; + rcPolyMeshDetail *detail_mesh = nullptr; + + _build_recast_navigation_mesh( + p_nav_mesh, +#ifdef TOOLS_ENABLED + ep, +#endif + hf, + chf, + cset, + poly_mesh, + detail_mesh, + vertices, + indices); + + rcFreeHeightField(hf); + hf = nullptr; + + rcFreeCompactHeightfield(chf); + chf = nullptr; + + rcFreeContourSet(cset); + cset = nullptr; + + rcFreePolyMesh(poly_mesh); + poly_mesh = nullptr; + + rcFreePolyMeshDetail(detail_mesh); + detail_mesh = nullptr; + } + +#ifdef TOOLS_ENABLED + if (ep) { + ep->step(TTR("Done!"), 11); + } + + if (ep) { + memdelete(ep); + } +#endif +} + +void NavigationMeshGenerator::clear(Ref<NavigationMesh> p_nav_mesh) { + if (p_nav_mesh.is_valid()) { + p_nav_mesh->clear_polygons(); + p_nav_mesh->set_vertices(Vector<Vector3>()); + } +} + +void NavigationMeshGenerator::_bind_methods() { + ClassDB::bind_method(D_METHOD("bake", "nav_mesh", "root_node"), &NavigationMeshGenerator::bake); + ClassDB::bind_method(D_METHOD("clear", "nav_mesh"), &NavigationMeshGenerator::clear); +} + +#endif diff --git a/modules/gdnavigation/navigation_mesh_generator.h b/modules/gdnavigation/navigation_mesh_generator.h new file mode 100644 index 0000000000..c5f7b2ab81 --- /dev/null +++ b/modules/gdnavigation/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-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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 Transform &p_xform, Vector<float> &p_verticies, Vector<int> &p_indices); + static void _add_faces(const PackedVector3Array &p_faces, const Transform &p_xform, Vector<float> &p_verticies, Vector<int> &p_indices); + static void _parse_geometry(Transform p_accumulated_transform, Node *p_node, Vector<float> &p_verticies, Vector<int> &p_indices, int p_generate_from, uint32_t p_collision_mask, bool p_recurse_children); + + static void _convert_detail_mesh_to_native_navigation_mesh(const rcPolyMeshDetail *p_detail_mesh, Ref<NavigationMesh> p_nav_mesh); + static void _build_recast_navigation_mesh( + Ref<NavigationMesh> p_nav_mesh, +#ifdef TOOLS_ENABLED + EditorProgress *ep, +#endif + rcHeightfield *hf, + rcCompactHeightfield *chf, + rcContourSet *cset, + rcPolyMesh *poly_mesh, + rcPolyMeshDetail *detail_mesh, + Vector<float> &vertices, + Vector<int> &indices); + +public: + static NavigationMeshGenerator *get_singleton(); + + NavigationMeshGenerator(); + ~NavigationMeshGenerator(); + + void bake(Ref<NavigationMesh> p_nav_mesh, Node *p_node); + void clear(Ref<NavigationMesh> p_nav_mesh); +}; + +#endif + +#endif // NAVIGATION_MESH_GENERATOR_H diff --git a/modules/gdnavigation/register_types.cpp b/modules/gdnavigation/register_types.cpp new file mode 100644 index 0000000000..088b26bf17 --- /dev/null +++ b/modules/gdnavigation/register_types.cpp @@ -0,0 +1,82 @@ +/*************************************************************************/ +/* register_types.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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/engine.h" +#include "gd_navigation_server.h" +#include "servers/navigation_server_3d.h" + +#ifndef _3D_DISABLED +#include "navigation_mesh_generator.h" +#endif + +#ifdef TOOLS_ENABLED +#include "navigation_mesh_editor_plugin.h" +#endif + +/** + @author AndreaCatania +*/ + +#ifndef _3D_DISABLED +NavigationMeshGenerator *_nav_mesh_generator = nullptr; +#endif + +NavigationServer3D *new_server() { + return memnew(GdNavigationServer); +} + +void register_gdnavigation_types() { + NavigationServer3DManager::set_default_server(new_server); + +#ifndef _3D_DISABLED + _nav_mesh_generator = memnew(NavigationMeshGenerator); + ClassDB::register_class<NavigationMeshGenerator>(); + Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationMeshGenerator", NavigationMeshGenerator::get_singleton())); +#endif + +#ifdef TOOLS_ENABLED + EditorPlugins::add_by_type<NavigationMeshEditorPlugin>(); + + ClassDB::APIType prev_api = ClassDB::get_current_api(); + ClassDB::set_current_api(ClassDB::API_EDITOR); + + ClassDB::set_current_api(prev_api); +#endif +} + +void unregister_gdnavigation_types() { +#ifndef _3D_DISABLED + if (_nav_mesh_generator) { + memdelete(_nav_mesh_generator); + } +#endif +} diff --git a/modules/gdnavigation/register_types.h b/modules/gdnavigation/register_types.h new file mode 100644 index 0000000000..cdbff1b937 --- /dev/null +++ b/modules/gdnavigation/register_types.h @@ -0,0 +1,41 @@ +/*************************************************************************/ +/* register_types.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +/** + @author AndreaCatania +*/ + +#ifndef GDNAVIGATION_REGISTER_TYPES_H +#define GDNAVIGATION_REGISTER_TYPES_H + +void register_gdnavigation_types(); +void unregister_gdnavigation_types(); + +#endif // GDNAVIGATION_REGISTER_TYPES_H diff --git a/modules/gdnavigation/rvo_agent.cpp b/modules/gdnavigation/rvo_agent.cpp new file mode 100644 index 0000000000..1e1bdbd07d --- /dev/null +++ b/modules/gdnavigation/rvo_agent.cpp @@ -0,0 +1,83 @@ +/*************************************************************************/ +/* rvo_agent.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ + +#include "rvo_agent.h" + +#include "nav_map.h" + +/** + @author AndreaCatania +*/ + +RvoAgent::RvoAgent() { + callback.id = ObjectID(); +} + +void RvoAgent::set_map(NavMap *p_map) { + map = p_map; +} + +bool RvoAgent::is_map_changed() { + if (map) { + bool is_changed = map->get_map_update_id() != map_update_id; + map_update_id = map->get_map_update_id(); + return is_changed; + } else { + return false; + } +} + +void RvoAgent::set_callback(ObjectID p_id, const StringName p_method, const Variant p_udata) { + callback.id = p_id; + callback.method = p_method; + callback.udata = p_udata; +} + +bool RvoAgent::has_callback() const { + return callback.id.is_valid(); +} + +void RvoAgent::dispatch_callback() { + if (callback.id.is_null()) { + return; + } + Object *obj = ObjectDB::get_instance(callback.id); + if (obj == nullptr) { + callback.id = ObjectID(); + } + + Callable::CallError responseCallError; + + callback.new_velocity = Vector3(agent.newVelocity_.x(), agent.newVelocity_.y(), agent.newVelocity_.z()); + + const Variant *vp[2] = { &callback.new_velocity, &callback.udata }; + int argc = (callback.udata.get_type() == Variant::NIL) ? 1 : 2; + obj->call(callback.method, vp, argc, responseCallError); +} diff --git a/modules/gdnavigation/rvo_agent.h b/modules/gdnavigation/rvo_agent.h new file mode 100644 index 0000000000..f5c579ba84 --- /dev/null +++ b/modules/gdnavigation/rvo_agent.h @@ -0,0 +1,77 @@ +/*************************************************************************/ +/* rvo_agent.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ +/* Copyright (c) 2014-2020 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.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; + +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 |