maxmustermann
/
godot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

- Integrated NavigationServer and Navigation2DServer. 

- Added Navigation Agents and Obstacles.
- Integrated Collision Avoidance.

This work has been kindly sponsored by IMVU.
This commit is contained in:
Andrea Catania 2020-01-10 12:22:34 +01:00
parent 8cd5102c65
commit e6be3f68da
103 changed files with 7087 additions and 2005 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
COPYRIGHT.txt
View File

@ -332,6 +332,11 @@ Comment: Recast
Copyright: 2009, Mikko Mononen
License: Zlib
Files: ./thirdparty/rvo2/
Comment: RVO2
Copyright: 2016, University of North Carolina at Chapel Hill
License: Apache 2.0
Files: ./thirdparty/squish/
Comment: libSquish
Copyright: 2006, Simon Brown

1
SConstruct
View File

@ -152,6 +152,7 @@ opts.Add(BoolVariable('builtin_opus', "Use the built-in Opus library", True))
opts.Add(BoolVariable('builtin_pcre2', "Use the built-in PCRE2 library", True))
opts.Add(BoolVariable('builtin_pcre2_with_jit', "Use JIT compiler for the built-in PCRE2 library", True))
opts.Add(BoolVariable('builtin_recast', "Use the built-in Recast library", True))
opts.Add(BoolVariable('builtin_rvo2', "Use the built-in RVO2 library", True))
opts.Add(BoolVariable('builtin_squish', "Use the built-in squish library", True))
opts.Add(BoolVariable('builtin_xatlas', "Use the built-in xatlas library", True))
opts.Add(BoolVariable('builtin_zlib', "Use the built-in zlib library", True))

4
editor/editor_node.cpp
View File

@ -62,6 +62,8 @@
#include "scene/gui/texture_progress.h"
#include "scene/gui/tool_button.h"
#include "scene/resources/packed_scene.h"
#include "servers/navigation_2d_server.h"
#include "servers/navigation_server.h"
#include "servers/physics_2d_server.h"
#include "editor/audio_stream_preview.h"
@ -5584,6 +5586,8 @@ EditorNode::EditorNode() {
VisualServer::get_singleton()->textures_keep_original(true);
VisualServer::get_singleton()->set_debug_generate_wireframes(true);
NavigationServer::get_singleton()->set_active(false); // no nav by default if editor
PhysicsServer::get_singleton()->set_active(false); // no physics by default if editor
Physics2DServer::get_singleton()->set_active(false); // no physics by default if editor
ScriptServer::set_scripting_enabled(false); // no scripting by default if editor

2
editor/plugins/mesh_instance_editor_plugin.cpp
View File

@ -32,7 +32,7 @@
#include "editor/editor_scale.h"
#include "scene/3d/collision_shape.h"
#include "scene/3d/navigation_mesh.h"
#include "scene/3d/navigation_mesh_instance.h"
#include "scene/3d/physics_body.h"
#include "scene/gui/box_container.h"
#include "spatial_editor_plugin.h"

2
editor/plugins/mesh_library_editor_plugin.cpp
View File

@ -34,7 +34,7 @@
#include "editor/editor_settings.h"
#include "main/main.h"
#include "scene/3d/mesh_instance.h"
#include "scene/3d/navigation_mesh.h"
#include "scene/3d/navigation_mesh_instance.h"
#include "scene/3d/physics_body.h"
#include "scene/main/viewport.h"
#include "scene/resources/packed_scene.h"

2
editor/spatial_editor_gizmos.cpp
View File

@ -41,7 +41,7 @@
#include "scene/3d/light.h"
#include "scene/3d/listener.h"
#include "scene/3d/mesh_instance.h"
#include "scene/3d/navigation_mesh.h"
#include "scene/3d/navigation_mesh_instance.h"
#include "scene/3d/particles.h"
#include "scene/3d/physics_joint.h"
#include "scene/3d/position_3d.h"

21
main/main.cpp
View File

@ -66,6 +66,8 @@
#include "servers/arvr_server.h"
#include "servers/audio_server.h"
#include "servers/camera_server.h"
#include "servers/navigation_2d_server.h"
#include "servers/navigation_server.h"
#include "servers/physics_2d_server.h"
#include "servers/physics_server.h"
#include "servers/register_server_types.h"
@ -103,6 +105,8 @@ static CameraServer *camera_server = NULL;
static ARVRServer *arvr_server = NULL;
static PhysicsServer *physics_server = NULL;
static Physics2DServer *physics_2d_server = NULL;
static NavigationServer *navigation_server = NULL;
static Navigation2DServer *navigation_2d_server = NULL;
// We error out if setup2() doesn't turn this true
static bool _start_success = false;
@ -197,6 +201,19 @@ void finalize_physics() {
memdelete(physics_2d_server);
}
void initialize_navigation_server() {
ERR_FAIL_COND(navigation_server != NULL);
navigation_server = NavigationServerManager::new_default_server();
navigation_2d_server = memnew(Navigation2DServer);
}
void finalize_navigation_server() {
memdelete(navigation_server);
navigation_server = NULL;
memdelete(navigation_2d_server);
navigation_2d_server = NULL;
}
//#define DEBUG_INIT
#ifdef DEBUG_INIT
#define MAIN_PRINT(m_txt) print_line(m_txt)
@ -1356,6 +1373,7 @@ Error Main::setup2(Thread::ID p_main_tid_override) {
camera_server = CameraServer::create();
initialize_physics();
initialize_navigation_server();
register_server_singletons();
register_driver_types();
@ -2011,6 +2029,8 @@ bool Main::iteration() {
message_queue->flush();
NavigationServer::get_singleton_mut()->step(frame_slice * time_scale);
PhysicsServer::get_singleton()->step(frame_slice * time_scale);
Physics2DServer::get_singleton()->end_sync();
@ -2202,6 +2222,7 @@ void Main::cleanup() {
OS::get_singleton()->finalize();
finalize_physics();
finalize_navigation_server();
if (packed_data)
memdelete(packed_data);

50
modules/gdnavigation/SCsub Normal file
View File

@ -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")

2
modules/recast/config.py → modules/gdnavigation/config.py
View File

@ -1,5 +1,5 @@
def can_build(env, platform):
return env['tools']
return True
def configure(env):
pass

472
modules/gdnavigation/gd_navigation_server.cpp Normal file
View File

@ -0,0 +1,472 @@
/*************************************************************************/
/* 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 submited 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() :
NavigationServer(),
active(true) {
commands_mutex = Mutex::create();
operations_mutex = Mutex::create();
}
GdNavigationServer::~GdNavigationServer() {}
void GdNavigationServer::add_command(SetCommand *command) const {
auto mut_this = const_cast<GdNavigationServer *>(this);
commands_mutex->lock();
mut_this->commands.push_back(command);
commands_mutex->unlock();
}
RID GdNavigationServer::map_create() const {
auto mut_this = const_cast<GdNavigationServer *>(this);
mut_this->operations_mutex->lock();
NavMap *space = memnew(NavMap);
RID rid = map_owner.make_rid(space);
space->set_self(rid);
mut_this->operations_mutex->unlock();
return rid;
}
COMMAND_2(map_set_active, RID, p_map, bool, p_active) {
NavMap *map = map_owner.get(p_map);
ERR_FAIL_COND(map == NULL);
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.get(p_map);
ERR_FAIL_COND_V(map == NULL, false);
return active_maps.find(map) >= 0;
}
COMMAND_2(map_set_up, RID, p_map, Vector3, p_up) {
NavMap *map = map_owner.get(p_map);
ERR_FAIL_COND(map == NULL);
map->set_up(p_up);
}
Vector3 GdNavigationServer::map_get_up(RID p_map) const {
NavMap *map = map_owner.get(p_map);
ERR_FAIL_COND_V(map == NULL, Vector3());
return map->get_up();
}
COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size) {
NavMap *map = map_owner.get(p_map);
ERR_FAIL_COND(map == NULL);
map->set_cell_size(p_cell_size);
}
real_t GdNavigationServer::map_get_cell_size(RID p_map) const {
NavMap *map = map_owner.get(p_map);
ERR_FAIL_COND_V(map == NULL, 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.get(p_map);
ERR_FAIL_COND(map == NULL);
map->set_edge_connection_margin(p_connection_margin);
}
real_t GdNavigationServer::map_get_edge_connection_margin(RID p_map) const {
NavMap *map = map_owner.get(p_map);
ERR_FAIL_COND_V(map == NULL, 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 {
NavMap *map = map_owner.get(p_map);
ERR_FAIL_COND_V(map == NULL, Vector<Vector3>());
return map->get_path(p_origin, p_destination, p_optimize);
}
RID GdNavigationServer::region_create() const {
auto mut_this = const_cast<GdNavigationServer *>(this);
mut_this->operations_mutex->lock();
NavRegion *reg = memnew(NavRegion);
RID rid = region_owner.make_rid(reg);
reg->set_self(rid);
mut_this->operations_mutex->unlock();
return rid;
}
COMMAND_2(region_set_map, RID, p_region, RID, p_map) {
NavRegion *region = region_owner.get(p_region);
ERR_FAIL_COND(region == NULL);
if (region->get_map() != NULL) {
if (region->get_map()->get_self() == p_map)
return; // Pointless
region->get_map()->remove_region(region);
region->set_map(NULL);
}
if (p_map.is_valid()) {
NavMap *map = map_owner.get(p_map);
ERR_FAIL_COND(map == NULL);
map->add_region(region);
region->set_map(map);
}
}
COMMAND_2(region_set_transform, RID, p_region, Transform, p_transform) {
NavRegion *region = region_owner.get(p_region);
ERR_FAIL_COND(region == NULL);
region->set_transform(p_transform);
}
COMMAND_2(region_set_navmesh, RID, p_region, Ref<NavigationMesh>, p_nav_mesh) {
NavRegion *region = region_owner.get(p_region);
ERR_FAIL_COND(region == NULL);
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 == NULL);
#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);
mut_this->operations_mutex->lock();
RvoAgent *agent = memnew(RvoAgent());
RID rid = agent_owner.make_rid(agent);
agent->set_self(rid);
mut_this->operations_mutex->unlock();
return rid;
}
COMMAND_2(agent_set_map, RID, p_agent, RID, p_map) {
RvoAgent *agent = agent_owner.get(p_agent);
ERR_FAIL_COND(agent == NULL);
if (agent->get_map()) {
if (agent->get_map()->get_self() == p_map)
return; // Pointless
agent->get_map()->remove_agent(agent);
}
agent->set_map(NULL);
if (p_map.is_valid()) {
NavMap *map = map_owner.get(p_map);
ERR_FAIL_COND(map == NULL);
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.get(p_agent);
ERR_FAIL_COND(agent == NULL);
agent->get_agent()->neighborDist_ = p_dist;
}
COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count) {
RvoAgent *agent = agent_owner.get(p_agent);
ERR_FAIL_COND(agent == NULL);
agent->get_agent()->maxNeighbors_ = p_count;
}
COMMAND_2(agent_set_time_horizon, RID, p_agent, real_t, p_time) {
RvoAgent *agent = agent_owner.get(p_agent);
ERR_FAIL_COND(agent == NULL);
agent->get_agent()->timeHorizon_ = p_time;
}
COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius) {
RvoAgent *agent = agent_owner.get(p_agent);
ERR_FAIL_COND(agent == NULL);
agent->get_agent()->radius_ = p_radius;
}
COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed) {
RvoAgent *agent = agent_owner.get(p_agent);
ERR_FAIL_COND(agent == NULL);
agent->get_agent()->maxSpeed_ = p_max_speed;
}
COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity) {
RvoAgent *agent = agent_owner.get(p_agent);
ERR_FAIL_COND(agent == NULL);
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.get(p_agent);
ERR_FAIL_COND(agent == NULL);
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.get(p_agent);
ERR_FAIL_COND(agent == NULL);
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.get(p_agent);
ERR_FAIL_COND(agent == NULL);
agent->get_agent()->ignore_y_ = p_ignore;
}
bool GdNavigationServer::agent_is_map_changed(RID p_agent) const {
RvoAgent *agent = agent_owner.get(p_agent);
ERR_FAIL_COND_V(agent == NULL, 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.get(p_agent);
ERR_FAIL_COND(agent == NULL);
agent->set_callback(p_receiver == NULL ? 0 : p_receiver->get_instance_id(), p_method, p_udata);
if (agent->get_map()) {
if (p_receiver == NULL) {
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.get(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(NULL);
}
// 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(NULL);
}
active_maps.erase(map);
map_owner.free(p_object);
memdelete(map);
} else if (region_owner.owns(p_object)) {
NavRegion *region = region_owner.get(p_object);
// Removes this region from the map if assigned
if (region->get_map() != NULL) {
region->get_map()->remove_region(region);
region->set_map(NULL);
}
region_owner.free(p_object);
memdelete(region);
} else if (agent_owner.owns(p_object)) {
RvoAgent *agent = agent_owner.get(p_object);
// Removes this agent from the map if assigned
if (agent->get_map() != NULL) {
agent->get_map()->remove_agent(agent);
agent->set_map(NULL);
}
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);
mut_this->operations_mutex->lock();
mut_this->active = p_active;
mut_this->operations_mutex->unlock();
}
void GdNavigationServer::step(real_t p_delta_time) {
if (!active) {
return;
}
// With c++ we can't be 100% sure this is called in single thread so use the mutex.
commands_mutex->lock();
operations_mutex->lock();
for (size_t i(0); i < commands.size(); i++) {
commands[i]->exec(this);
memdelete(commands[i]);
}
commands.clear();
operations_mutex->unlock();
commands_mutex->unlock();
// These are internal operations so don't need to be shielded.
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

134
modules/gdnavigation/gd_navigation_server.h Normal file
View File

@ -0,0 +1,134 @@
/*************************************************************************/
/* 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 "servers/navigation_server.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;
class Mutex;
struct SetCommand {
virtual ~SetCommand() {}
virtual void exec(GdNavigationServer *server) = 0;
};
class GdNavigationServer : public NavigationServer {
Mutex *commands_mutex;
/// Mutex used to make any operation threadsafe.
Mutex *operations_mutex;
std::vector<SetCommand *> commands;
mutable RID_Owner<NavMap> map_owner;
mutable RID_Owner<NavRegion> region_owner;
mutable RID_Owner<RvoAgent> agent_owner;
bool active;
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 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;
virtual void step(real_t p_delta_time);
};
#undef COMMAND_1
#undef COMMAND_2
#undef COMMAND_4_DEF
#endif // GD_NAVIGATION_SERVER_H

665
modules/gdnavigation/nav_map.cpp Normal file
View File

@ -0,0 +1,665 @@
/*************************************************************************/
/* rvo_space.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
NavMap::NavMap() :
up(0, 1, 0),
cell_size(0.3),
edge_connection_margin(5.0),
regenerate_polygons(true),
regenerate_links(true),
agents_dirty(false),
deltatime(0.0),
map_update_id(0) {}
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 = NULL;
const gd::Polygon *end_poly = NULL;
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 = NULL;
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 = Geometry::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 == NULL) {
// 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 = NULL;
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 != NULL; 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 (CLOCK_TANGENT(apex_point,left,(left+right)*0.5).dot(up) < 0){
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 = NULL;
}
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>();
}
void NavMap::add_region(NavRegion *p_region) {
regions.push_back(p_region);
regenerate_links = true;
}
void NavMap::remove_region(NavRegion *p_region) {
regions.push_back(p_region);
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 = NULL;
c.B_edge = -1;
connections[ek] = c;
} else if (connection->get().B == NULL) {
CRASH_COND(connection->get().A == NULL); // 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.
}
}
}
// 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 == NULL) {
CRASH_COND(connection_element->get().A == NULL); // 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 tollerance
#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 alligned?
&& 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);
}
}
}
}
}

137
modules/gdnavigation/nav_map.h Normal file
View File

@ -0,0 +1,137 @@
/*************************************************************************/
/* rvo_space.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;
/// To find the polygons edges the vertices are displaced in a grid where
/// each cell has the following cell_size.
real_t cell_size;
/// This value is used to detect the near edges to connect.
real_t edge_connection_margin;
bool regenerate_polygons;
bool regenerate_links;
std::vector<NavRegion *> regions;
/// Map polygons
std::vector<gd::Polygon> polygons;
/// Rvo world
RVO::KdTree rvo;
/// Is agent array modified?
bool agents_dirty;
/// All the Agents (even the controlled one)
std::vector<RvoAgent *> agents;
/// Controlled agents
std::vector<RvoAgent *> controlled_agents;
/// Physics delta time
real_t deltatime;
/// Change the id each time the map is updated.
uint32_t map_update_id;
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;
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

136
modules/gdnavigation/nav_region.cpp Normal file
View File

@ -0,0 +1,136 @@
/*************************************************************************/
/* 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. */
/*************************************************************************/
#include "nav_region.h"
#include "nav_map.h"
/**
@author AndreaCatania
*/
NavRegion::NavRegion() :
map(NULL),
polygons_dirty(true) {
}
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 == NULL) {
return;
}
if (mesh.is_null())
return;
PoolVector<Vector3> vertices = mesh->get_vertices();
int len = vertices.size();
if (len == 0)
return;
PoolVector<Vector3>::Read vertices_r = vertices.read();
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());
}
}
}

89
modules/gdnavigation/nav_region.h Normal file
View File

@ -0,0 +1,89 @@
/*************************************************************************/
/* 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. */
/*************************************************************************/
#ifndef NAV_REGION_H
#define NAV_REGION_H
#include "nav_rid.h"
#include "nav_utils.h"
#include "scene/3d/navigation.h"
#include <vector>
/**
@author AndreaCatania
*/
class NavMap;
class NavRegion;
class NavRegion : public NavRid {
NavMap *map;
Transform transform;
Ref<NavigationMesh> mesh;
bool polygons_dirty;
/// 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

48
modules/gdnavigation/nav_rid.h Normal file
View File

@ -0,0 +1,48 @@
/*************************************************************************/
/* rvo_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 : public RID_Data {
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

169
modules/gdnavigation/nav_utils.h Normal file
View File

@ -0,0 +1,169 @@
/*************************************************************************/
/* 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;
/// Other Polygon
Polygon *other_polygon;
/// The other `Polygon` at this edge id has this `Polygon`.
int other_edge;
Edge() {
this_edge = -1;
other_polygon = NULL;
other_edge = -1;
}
};
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;
int A_edge;
Polygon *B;
int B_edge;
Connection() {
A = NULL;
B = NULL;
A_edge = -1;
B_edge = -1;
}
};
struct NavigationPoly {
uint32_t self_id;
/// This poly.
const Polygon *poly;
/// The previous navigation poly (id in the `navigation_poly` array).
int prev_navigation_poly_id;
/// The edge id in this `Poly` to reach the `prev_navigation_poly_id`.
uint32_t back_navigation_edge;
/// The entry location of this poly.
Vector3 entry;
/// The distance to the destination.
float traveled_distance;
NavigationPoly(const Polygon *p_poly) :
self_id(0),
poly(p_poly),
prev_navigation_poly_id(-1),
back_navigation_edge(0),
traveled_distance(0.0) {
}
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

15
modules/recast/navigation_mesh_editor_plugin.cpp → modules/gdnavigation/navigation_mesh_editor_plugin.cpp
View File

@ -28,10 +28,12 @@
/* 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.h"
#include "scene/gui/box_container.h"
@ -57,15 +59,14 @@ void NavigationMeshEditor::_bake_pressed() {
button_bake->set_pressed(false);
ERR_FAIL_COND(!node);
const String conf_warning = node->get_configuration_warning();
if (!conf_warning.empty()) {
err_dialog->set_text(conf_warning);
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_minsize();
return;
}
EditorNavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh());
EditorNavigationMeshGenerator::get_singleton()->bake(node->get_navigation_mesh(), node);
NavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh());
NavigationMeshGenerator::get_singleton()->bake(node->get_navigation_mesh(), node);
node->update_gizmo();
}
@ -73,7 +74,7 @@ void NavigationMeshEditor::_bake_pressed() {
void NavigationMeshEditor::_clear_pressed() {
if (node)
EditorNavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh());
NavigationMeshGenerator::get_singleton()->clear(node->get_navigation_mesh());
button_bake->set_pressed(false);
bake_info->set_text("");
@ -160,3 +161,5 @@ NavigationMeshEditorPlugin::NavigationMeshEditorPlugin(EditorNode *p_node) {
NavigationMeshEditorPlugin::~NavigationMeshEditorPlugin() {
}
#endif

13
modules/recast/navigation_mesh_editor_plugin.h → modules/gdnavigation/navigation_mesh_editor_plugin.h
View File

@ -28,12 +28,15 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef NAVIGATION_MESH_GENERATOR_PLUGIN_H
#define NAVIGATION_MESH_GENERATOR_PLUGIN_H
#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"
#include "navigation_mesh_generator.h"
class NavigationMeshInstance;
class NavigationMeshEditor : public Control {
friend class NavigationMeshEditorPlugin;
@ -81,4 +84,6 @@ public:
~NavigationMeshEditorPlugin();
};
#endif // NAVIGATION_MESH_GENERATOR_PLUGIN_H
#endif
#endif

139
modules/recast/navigation_mesh_generator.cpp → modules/gdnavigation/navigation_mesh_generator.cpp
View File

@ -28,11 +28,12 @@
/* 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 "editor/editor_settings.h"
#include "scene/3d/collision_shape.h"
#include "scene/3d/mesh_instance.h"
#include "scene/3d/physics_body.h"
@ -47,6 +48,11 @@
#include "scene/resources/sphere_shape.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
@ -54,15 +60,15 @@
#include "modules/gridmap/grid_map.h"
#endif
EditorNavigationMeshGenerator *EditorNavigationMeshGenerator::singleton = NULL;
NavigationMeshGenerator *NavigationMeshGenerator::singleton = NULL;
void EditorNavigationMeshGenerator::_add_vertex(const Vector3 &p_vec3, Vector<float> &p_verticies) {
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 EditorNavigationMeshGenerator::_add_mesh(const Ref<Mesh> &p_mesh, const Transform &p_xform, Vector<float> &p_verticies, Vector<int> &p_indices) {
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++) {
@ -117,7 +123,7 @@ void EditorNavigationMeshGenerator::_add_mesh(const Ref<Mesh> &p_mesh, const Tra
}
}
void EditorNavigationMeshGenerator::_add_faces(const PoolVector3Array &p_faces, const Transform &p_xform, Vector<float> &p_verticies, Vector<int> &p_indices) {
void NavigationMeshGenerator::_add_faces(const PoolVector3Array &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;
@ -132,7 +138,7 @@ void EditorNavigationMeshGenerator::_add_faces(const PoolVector3Array &p_faces,
}
}
void EditorNavigationMeshGenerator::_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) {
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<MeshInstance>(p_node) && p_generate_from != NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS) {
@ -271,7 +277,7 @@ void EditorNavigationMeshGenerator::_parse_geometry(Transform p_accumulated_tran
}
}
void EditorNavigationMeshGenerator::_convert_detail_mesh_to_native_navigation_mesh(const rcPolyMeshDetail *p_detail_mesh, Ref<NavigationMesh> p_nav_mesh) {
void NavigationMeshGenerator::_convert_detail_mesh_to_native_navigation_mesh(const rcPolyMeshDetail *p_detail_mesh, Ref<NavigationMesh> p_nav_mesh) {
PoolVector<Vector3> nav_vertices;
@ -299,11 +305,24 @@ void EditorNavigationMeshGenerator::_convert_detail_mesh_to_native_navigation_me
}
}
void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Ref<NavigationMesh> p_nav_mesh, EditorProgress *ep,
rcHeightfield *hf, rcCompactHeightfield *chf, rcContourSet *cset, rcPolyMesh *poly_mesh, rcPolyMeshDetail *detail_mesh,
Vector<float> &vertices, Vector<int> &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;
ep->step(TTR("Setting up Configuration..."), 1);
#ifdef TOOLS_ENABLED
if (ep)
ep->step(TTR("Setting up Configuration..."), 1);
#endif
const float *verts = vertices.ptr();
const int nverts = vertices.size() / 3;
@ -337,16 +356,25 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Ref<Navigation
cfg.bmax[1] = bmax[1];
cfg.bmax[2] = bmax[2];
ep->step(TTR("Calculating grid size..."), 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);
ep->step(TTR("Creating heightfield..."), 3);
#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));
ep->step(TTR("Marking walkable triangles..."), 4);
#ifdef TOOLS_ENABLED
if (ep)
ep->step(TTR("Marking walkable triangles..."), 4);
#endif
{
Vector<unsigned char> tri_areas;
tri_areas.resize(ntris);
@ -366,7 +394,10 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Ref<Navigation
if (p_nav_mesh->get_filter_walkable_low_height_spans())
rcFilterWalkableLowHeightSpans(&ctx, cfg.walkableHeight, *hf);
ep->step(TTR("Constructing compact heightfield..."), 5);
#ifdef TOOLS_ENABLED
if (ep)
ep->step(TTR("Constructing compact heightfield..."), 5);
#endif
chf = rcAllocCompactHeightfield();
@ -376,10 +407,18 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Ref<Navigation
rcFreeHeightField(hf);
hf = 0;
ep->step(TTR("Eroding walkable area..."), 6);
#ifdef TOOLS_ENABLED
if (ep)
ep->step(TTR("Eroding walkable area..."), 6);
#endif
ERR_FAIL_COND(!rcErodeWalkableArea(&ctx, cfg.walkableRadius, *chf));
ep->step(TTR("Partitioning..."), 7);
#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));
@ -389,14 +428,20 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Ref<Navigation
ERR_FAIL_COND(!rcBuildLayerRegions(&ctx, *chf, 0, cfg.minRegionArea));
}
ep->step(TTR("Creating contours..."), 8);
#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));
ep->step(TTR("Creating polymesh..."), 9);
#ifdef TOOLS_ENABLED
if (ep)
ep->step(TTR("Creating polymesh..."), 9);
#endif
poly_mesh = rcAllocPolyMesh();
ERR_FAIL_COND(!poly_mesh);
@ -411,7 +456,10 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Ref<Navigation
rcFreeContourSet(cset);
cset = 0;
ep->step(TTR("Converting to native navigation mesh..."), 10);
#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);
@ -421,23 +469,30 @@ void EditorNavigationMeshGenerator::_build_recast_navigation_mesh(Ref<Navigation
detail_mesh = 0;
}
EditorNavigationMeshGenerator *EditorNavigationMeshGenerator::get_singleton() {
NavigationMeshGenerator *NavigationMeshGenerator::get_singleton() {
return singleton;
}
EditorNavigationMeshGenerator::EditorNavigationMeshGenerator() {
NavigationMeshGenerator::NavigationMeshGenerator() {
singleton = this;
}
EditorNavigationMeshGenerator::~EditorNavigationMeshGenerator() {
NavigationMeshGenerator::~NavigationMeshGenerator() {
}
void EditorNavigationMeshGenerator::bake(Ref<NavigationMesh> p_nav_mesh, Node *p_node) {
void NavigationMeshGenerator::bake(Ref<NavigationMesh> p_nav_mesh, Node *p_node) {
ERR_FAIL_COND(!p_nav_mesh.is_valid());
EditorProgress ep("bake", TTR("Navigation Mesh Generator Setup:"), 11);
ep.step(TTR("Parsing Geometry..."), 0);
#ifdef TOOLS_ENABLED
EditorProgress *ep(NULL);
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;
@ -466,7 +521,18 @@ void EditorNavigationMeshGenerator::bake(Ref<NavigationMesh> p_nav_mesh, Node *p
rcPolyMesh *poly_mesh = NULL;
rcPolyMeshDetail *detail_mesh = NULL;
_build_recast_navigation_mesh(p_nav_mesh, &ep, hf, chf, cset, poly_mesh, detail_mesh, vertices, indices);
_build_recast_navigation_mesh(
p_nav_mesh,
#ifdef TOOLS_ENABLED
ep,
#endif
hf,
chf,
cset,
poly_mesh,
detail_mesh,
vertices,
indices);
rcFreeHeightField(hf);
hf = 0;
@ -483,17 +549,26 @@ void EditorNavigationMeshGenerator::bake(Ref<NavigationMesh> p_nav_mesh, Node *p
rcFreePolyMeshDetail(detail_mesh);
detail_mesh = 0;
}
ep.step(TTR("Done!"), 11);
#ifdef TOOLS_ENABLED
if (ep)
ep->step(TTR("Done!"), 11);
if (ep)
memdelete(ep);
#endif
}
void EditorNavigationMeshGenerator::clear(Ref<NavigationMesh> p_nav_mesh) {
void NavigationMeshGenerator::clear(Ref<NavigationMesh> p_nav_mesh) {
if (p_nav_mesh.is_valid()) {
p_nav_mesh->clear_polygons();
p_nav_mesh->set_vertices(PoolVector<Vector3>());
}
}
void EditorNavigationMeshGenerator::_bind_methods() {
ClassDB::bind_method(D_METHOD("bake", "nav_mesh", "root_node"), &EditorNavigationMeshGenerator::bake);
ClassDB::bind_method(D_METHOD("clear", "nav_mesh"), &EditorNavigationMeshGenerator::clear);
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

38
modules/recast/navigation_mesh_generator.h → modules/gdnavigation/navigation_mesh_generator.h
View File

@ -31,15 +31,20 @@
#ifndef NAVIGATION_MESH_GENERATOR_H
#define NAVIGATION_MESH_GENERATOR_H
#include "editor/editor_node.h"
#include "scene/3d/navigation_mesh.h"
#ifndef _3D_DISABLED
#include "scene/3d/navigation_mesh_instance.h"
#include <Recast.h>
class EditorNavigationMeshGenerator : public Object {
GDCLASS(EditorNavigationMeshGenerator, Object);
#ifdef TOOLS_ENABLED
struct EditorProgress;
#endif
static EditorNavigationMeshGenerator *singleton;
class NavigationMeshGenerator : public Object {
GDCLASS(NavigationMeshGenerator, Object);
static NavigationMeshGenerator *singleton;
protected:
static void _bind_methods();
@ -50,18 +55,29 @@ protected:
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, EditorProgress *ep,
rcHeightfield *hf, rcCompactHeightfield *chf, rcContourSet *cset, rcPolyMesh *poly_mesh,
rcPolyMeshDetail *detail_mesh, Vector<float> &vertices, Vector<int> &indices);
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 EditorNavigationMeshGenerator *get_singleton();
static NavigationMeshGenerator *get_singleton();
EditorNavigationMeshGenerator();
~EditorNavigationMeshGenerator();
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

47
modules/recast/register_types.cpp → modules/gdnavigation/register_types.cpp
View File

@ -30,30 +30,51 @@
#include "register_types.h"
#include "navigation_mesh_editor_plugin.h"
#include "core/engine.h"
#include "gd_navigation_server.h"
#include "servers/navigation_server.h"
#ifdef TOOLS_ENABLED
EditorNavigationMeshGenerator *_nav_mesh_generator = NULL;
#ifndef _3D_DISABLED
#include "navigation_mesh_generator.h"
#endif
void register_recast_types() {
#ifdef TOOLS_ENABLED
#include "navigation_mesh_editor_plugin.h"
#endif
/**
@author AndreaCatania
*/
#ifndef _3D_DISABLED
NavigationMeshGenerator *_nav_mesh_generator = NULL;
#endif
NavigationServer *new_server() {
return memnew(GdNavigationServer);
}
void register_gdnavigation_types() {
NavigationServerManager::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);
EditorPlugins::add_by_type<NavigationMeshEditorPlugin>();
_nav_mesh_generator = memnew(EditorNavigationMeshGenerator);
ClassDB::register_class<EditorNavigationMeshGenerator>();
Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationMeshGenerator", EditorNavigationMeshGenerator::get_singleton()));
ClassDB::set_current_api(prev_api);
#endif
}
void unregister_recast_types() {
#ifdef TOOLS_ENABLED
void unregister_gdnavigation_types() {
#ifndef _3D_DISABLED
if (_nav_mesh_generator) {
memdelete(_nav_mesh_generator);
}

8
modules/recast/register_types.h → modules/gdnavigation/register_types.h
View File

@ -28,5 +28,9 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
void register_recast_types();
void unregister_recast_types();
/**
@author AndreaCatania
*/
void register_gdnavigation_types();
void unregister_gdnavigation_types();

84
modules/gdnavigation/rvo_agent.cpp Normal file
View File

@ -0,0 +1,84 @@
/*************************************************************************/
/* 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() :
map(NULL) {
callback.id = ObjectID(0);
}
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 != 0;
}
void RvoAgent::dispatch_callback() {
if (callback.id == 0) {
return;
}
Object *obj = ObjectDB::get_instance(callback.id);
if (obj == NULL) {
callback.id = ObjectID(0);
}
Variant::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);
}

77
modules/gdnavigation/rvo_agent.h Normal file
View File

@ -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;
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

41
modules/gridmap/grid_map.cpp
View File

@ -36,6 +36,7 @@
#include "scene/resources/mesh_library.h"
#include "scene/resources/surface_tool.h"
#include "scene/scene_string_names.h"
#include "servers/navigation_server.h"
#include "servers/visual_server.h"
bool GridMap::_set(const StringName &p_name, const Variant &p_value) {
@ -418,12 +419,10 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
}
//erase navigation
if (navigation) {
for (Map<IndexKey, Octant::NavMesh>::Element *E = g.navmesh_ids.front(); E; E = E->next()) {
navigation->navmesh_remove(E->get().id);
}
g.navmesh_ids.clear();
for (Map<IndexKey, Octant::NavMesh>::Element *E = g.navmesh_ids.front(); E; E = E->next()) {
NavigationServer::get_singleton()->free(E->get().region);
}
g.navmesh_ids.clear();
//erase multimeshes
@ -498,9 +497,11 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
nm.xform = xform * mesh_library->get_item_navmesh_transform(c.item);
if (navigation) {
nm.id = navigation->navmesh_add(navmesh, xform, this);
} else {
nm.id = -1;
RID region = NavigationServer::get_singleton()->region_create();
NavigationServer::get_singleton()->region_set_navmesh(region, navmesh);
NavigationServer::get_singleton()->region_set_transform(region, navigation->get_global_transform() * nm.xform);
NavigationServer::get_singleton()->region_set_map(region, navigation->get_rid());
nm.region = region;
}
g.navmesh_ids[E->get()] = nm;
}
@ -591,10 +592,14 @@ void GridMap::_octant_enter_world(const OctantKey &p_key) {
if (navigation && mesh_library.is_valid()) {
for (Map<IndexKey, Octant::NavMesh>::Element *F = g.navmesh_ids.front(); F; F = F->next()) {
if (cell_map.has(F->key()) && F->get().id < 0) {
if (cell_map.has(F->key()) && F->get().region.is_valid() == false) {
Ref<NavigationMesh> nm = mesh_library->get_item_navmesh(cell_map[F->key()].item);
if (nm.is_valid()) {
F->get().id = navigation->navmesh_add(nm, F->get().xform, this);
RID region = NavigationServer::get_singleton()->region_create();
NavigationServer::get_singleton()->region_set_navmesh(region, nm);
NavigationServer::get_singleton()->region_set_transform(region, navigation->get_global_transform() * F->get().xform);
NavigationServer::get_singleton()->region_set_map(region, navigation->get_rid());
F->get().region = region;
}
}
}
@ -620,9 +625,9 @@ void GridMap::_octant_exit_world(const OctantKey &p_key) {
if (navigation) {
for (Map<IndexKey, Octant::NavMesh>::Element *F = g.navmesh_ids.front(); F; F = F->next()) {
if (F->get().id >= 0) {
navigation->navmesh_remove(F->get().id);
F->get().id = -1;
if (F->get().region.is_valid()) {
NavigationServer::get_singleton()->free(F->get().region);
F->get().region = RID();
}
}
}
@ -640,13 +645,11 @@ void GridMap::_octant_clean_up(const OctantKey &p_key) {
PhysicsServer::get_singleton()->free(g.static_body);
//erase navigation
if (navigation) {
for (Map<IndexKey, Octant::NavMesh>::Element *E = g.navmesh_ids.front(); E; E = E->next()) {
navigation->navmesh_remove(E->get().id);
}
g.navmesh_ids.clear();
// Erase navigation
for (Map<IndexKey, Octant::NavMesh>::Element *E = g.navmesh_ids.front(); E; E = E->next()) {
NavigationServer::get_singleton()->free(E->get().region);
}
g.navmesh_ids.clear();
//erase multimeshes

2
modules/gridmap/grid_map.h
View File

@ -91,7 +91,7 @@ class GridMap : public Spatial {
struct Octant {
struct NavMesh {
int id;
RID region;
Transform xform;
};

33
modules/recast/SCsub
View File

@ -1,33 +0,0 @@
#!/usr/bin/env python
Import('env')
Import('env_modules')
env_recast = env_modules.Clone()
# 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_recast.Prepend(CPPPATH=[thirdparty_dir + "/Include"])
env_thirdparty = env_recast.Clone()
env_thirdparty.disable_warnings()
env_thirdparty.add_source_files(env.modules_sources, thirdparty_sources)
# Godot source files
env_recast.add_source_files(env.modules_sources, "*.cpp")

716
scene/2d/navigation_2d.cpp
View File

@ -29,710 +29,52 @@
/*************************************************************************/
#include "navigation_2d.h"
#include "servers/navigation_2d_server.h"
#define USE_ENTRY_POINT
void Navigation2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_rid"), &Navigation2D::get_rid);
void Navigation2D::_navpoly_link(int p_id) {
ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation2D::get_simple_path, DEFVAL(true));
ERR_FAIL_COND(!navpoly_map.has(p_id));
NavMesh &nm = navpoly_map[p_id];
ERR_FAIL_COND(nm.linked);
ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &Navigation2D::set_cell_size);
ClassDB::bind_method(D_METHOD("get_cell_size"), &Navigation2D::get_cell_size);
PoolVector<Vector2> vertices = nm.navpoly->get_vertices();
int len = vertices.size();
if (len == 0)
return;
ClassDB::bind_method(D_METHOD("set_edge_connection_margin", "margin"), &Navigation2D::set_edge_connection_margin);
ClassDB::bind_method(D_METHOD("get_edge_connection_margin"), &Navigation2D::get_edge_connection_margin);
PoolVector<Vector2>::Read r = vertices.read();
ADD_PROPERTY(PropertyInfo(Variant::REAL, "cell_size"), "set_cell_size", "get_cell_size");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "edge_connection_margin"), "set_edge_connection_margin", "get_edge_connection_margin");
}
for (int i = 0; i < nm.navpoly->get_polygon_count(); i++) {
void Navigation2D::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_READY: {
Navigation2DServer::get_singleton()->map_set_active(map, true);
} break;
case NOTIFICATION_EXIT_TREE: {
//build
List<Polygon>::Element *P = nm.polygons.push_back(Polygon());
Polygon &p = P->get();
p.owner = &nm;
Vector<int> poly = nm.navpoly->get_polygon(i);
int plen = poly.size();
const int *indices = poly.ptr();
bool valid = true;
p.edges.resize(plen);
Vector2 center;
float sum = 0;
for (int j = 0; j < plen; j++) {
int idx = indices[j];
if (idx < 0 || idx >= len) {
valid = false;
break;
}
Polygon::Edge e;
Vector2 ep = nm.xform.xform(r[idx]);
center += ep;
e.point = _get_point(ep);
p.edges.write[j] = e;
int idxn = indices[(j + 1) % plen];
if (idxn < 0 || idxn >= len) {
valid = false;
break;
}
Vector2 epn = nm.xform.xform(r[idxn]);
sum += (epn.x - ep.x) * (epn.y + ep.y);
}
p.clockwise = sum > 0;
if (!valid) {
nm.polygons.pop_back();
ERR_CONTINUE(!valid);
}
p.center = center / plen;
//connect
for (int j = 0; j < plen; j++) {
int next = (j + 1) % plen;
EdgeKey ek(p.edges[j].point, p.edges[next].point);
Map<EdgeKey, Connection>::Element *C = connections.find(ek);
if (!C) {
Connection c;
c.A = &p;
c.A_edge = j;
c.B = NULL;
c.B_edge = -1;
connections[ek] = c;
} else {
if (C->get().B != NULL) {
ConnectionPending pending;
pending.polygon = &p;
pending.edge = j;
p.edges.write[j].P = C->get().pending.push_back(pending);
continue;
}
C->get().B = &p;
C->get().B_edge = j;
C->get().A->edges.write[C->get().A_edge].C = &p;
C->get().A->edges.write[C->get().A_edge].C_edge = j;
p.edges.write[j].C = C->get().A;
p.edges.write[j].C_edge = C->get().A_edge;
//connection successful.
}
}
Navigation2DServer::get_singleton()->map_set_active(map, false);
} break;
}
nm.linked = true;
}
void Navigation2D::_navpoly_unlink(int p_id) {
ERR_FAIL_COND(!navpoly_map.has(p_id));
NavMesh &nm = navpoly_map[p_id];
ERR_FAIL_COND(!nm.linked);
for (List<Polygon>::Element *E = nm.polygons.front(); E; E = E->next()) {
Polygon &p = E->get();
int ec = p.edges.size();
Polygon::Edge *edges = p.edges.ptrw();
for (int i = 0; i < ec; i++) {
int next = (i + 1) % ec;
EdgeKey ek(edges[i].point, edges[next].point);
Map<EdgeKey, Connection>::Element *C = connections.find(ek);
ERR_CONTINUE(!C);
if (edges[i].P) {
C->get().pending.erase(edges[i].P);
edges[i].P = NULL;
} else if (C->get().B) {
//disconnect
C->get().B->edges.write[C->get().B_edge].C = NULL;
C->get().B->edges.write[C->get().B_edge].C_edge = -1;
C->get().A->edges.write[C->get().A_edge].C = NULL;
C->get().A->edges.write[C->get().A_edge].C_edge = -1;
if (C->get().A == &E->get()) {
C->get().A = C->get().B;
C->get().A_edge = C->get().B_edge;
}
C->get().B = NULL;
C->get().B_edge = -1;
if (C->get().pending.size()) {
//reconnect if something is pending
ConnectionPending cp = C->get().pending.front()->get();
C->get().pending.pop_front();
C->get().B = cp.polygon;
C->get().B_edge = cp.edge;
C->get().A->edges.write[C->get().A_edge].C = cp.polygon;
C->get().A->edges.write[C->get().A_edge].C_edge = cp.edge;
cp.polygon->edges.write[cp.edge].C = C->get().A;
cp.polygon->edges.write[cp.edge].C_edge = C->get().A_edge;
cp.polygon->edges.write[cp.edge].P = NULL;
}
} else {
connections.erase(C);
//erase
}
}
}
nm.polygons.clear();
nm.linked = false;
void Navigation2D::set_cell_size(float p_cell_size) {
cell_size = p_cell_size;
Navigation2DServer::get_singleton()->map_set_cell_size(map, cell_size);
}
int Navigation2D::navpoly_add(const Ref<NavigationPolygon> &p_mesh, const Transform2D &p_xform, Object *p_owner) {
int id = last_id++;
NavMesh nm;
nm.linked = false;
nm.navpoly = p_mesh;
nm.xform = p_xform;
nm.owner = p_owner;
navpoly_map[id] = nm;
_navpoly_link(id);
return id;
}
void Navigation2D::navpoly_set_transform(int p_id, const Transform2D &p_xform) {
ERR_FAIL_COND(!navpoly_map.has(p_id));
NavMesh &nm = navpoly_map[p_id];
if (nm.xform == p_xform)
return; //bleh
_navpoly_unlink(p_id);
nm.xform = p_xform;
_navpoly_link(p_id);
}
void Navigation2D::navpoly_remove(int p_id) {
ERR_FAIL_COND(!navpoly_map.has(p_id));
_navpoly_unlink(p_id);
navpoly_map.erase(p_id);
void Navigation2D::set_edge_connection_margin(float p_edge_connection_margin) {
edge_connection_margin = p_edge_connection_margin;
Navigation2DServer::get_singleton()->map_set_edge_connection_margin(map, edge_connection_margin);
}
Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize) {
Polygon *begin_poly = NULL;
Polygon *end_poly = NULL;
Vector2 begin_point;
Vector2 end_point;
float begin_d = 1e20;
float end_d = 1e20;
//look for point inside triangle
for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
if (!E->get().linked)
continue;
for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
Polygon &p = F->get();
if (begin_d || end_d) {
for (int i = 2; i < p.edges.size(); i++) {
if (begin_d > 0) {
if (Geometry::is_point_in_triangle(p_start, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
begin_poly = &p;
begin_point = p_start;
begin_d = 0;
if (end_d == 0)
break;
}
}
if (end_d > 0) {
if (Geometry::is_point_in_triangle(p_end, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
end_poly = &p;
end_point = p_end;
end_d = 0;
if (begin_d == 0)
break;
}
}
}
}
p.prev_edge = -1;
}
}
//start or end not inside triangle.. look for closest segment :|
if (begin_d || end_d) {
for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
if (!E->get().linked)
continue;
for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
Polygon &p = F->get();
int es = p.edges.size();
for (int i = 0; i < es; i++) {
Vector2 edge[2] = {
_get_vertex(p.edges[i].point),
_get_vertex(p.edges[(i + 1) % es].point)
};
if (begin_d > 0) {
Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_start, edge);
float d = spoint.distance_to(p_start);
if (d < begin_d) {
begin_poly = &p;
begin_point = spoint;
begin_d = d;
}
}
if (end_d > 0) {
Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_end, edge);
float d = spoint.distance_to(p_end);
if (d < end_d) {
end_poly = &p;
end_point = spoint;
end_d = d;
}
}
}
}
}
}
if (!begin_poly || !end_poly) {
return Vector<Vector2>(); //no path
}
if (begin_poly == end_poly) {
Vector<Vector2> path;
path.resize(2);
path.write[0] = begin_point;
path.write[1] = end_point;
return path;
}
bool found_route = false;
List<Polygon *> open_list;
begin_poly->entry = p_start;
for (int i = 0; i < begin_poly->edges.size(); i++) {
if (begin_poly->edges[i].C) {
begin_poly->edges[i].C->prev_edge = begin_poly->edges[i].C_edge;
#ifdef USE_ENTRY_POINT
Vector2 edge[2] = {
_get_vertex(begin_poly->edges[i].point),
_get_vertex(begin_poly->edges[(i + 1) % begin_poly->edges.size()].point)
};
Vector2 entry = Geometry::get_closest_point_to_segment_2d(begin_poly->entry, edge);
begin_poly->edges[i].C->distance = begin_poly->entry.distance_to(entry);
begin_poly->edges[i].C->entry = entry;
#else
begin_poly->edges[i].C->distance = begin_poly->center.distance_to(begin_poly->edges[i].C->center);
#endif
open_list.push_back(begin_poly->edges[i].C);
if (begin_poly->edges[i].C == end_poly) {
found_route = true;
}
}
}
while (!found_route) {
if (open_list.size() == 0) {
break;
}
//check open list
List<Polygon *>::Element *least_cost_poly = NULL;
float least_cost = 1e30;
//this could be faster (cache previous results)
for (List<Polygon *>::Element *E = open_list.front(); E; E = E->next()) {
Polygon *p = E->get();
float cost = p->distance;
#ifdef USE_ENTRY_POINT
int es = p->edges.size();
float shortest_distance = 1e30;
for (int i = 0; i < es; i++) {
Polygon::Edge &e = p->edges.write[i];
if (!e.C)
continue;
Vector2 edge[2] = {
_get_vertex(p->edges[i].point),
_get_vertex(p->edges[(i + 1) % es].point)
};
Vector2 edge_point = Geometry::get_closest_point_to_segment_2d(p->entry, edge);
float dist = p->entry.distance_to(edge_point);
if (dist < shortest_distance)
shortest_distance = dist;
}
cost += shortest_distance;
#else
cost += p->center.distance_to(end_point);
#endif
if (cost < least_cost) {
least_cost_poly = E;
least_cost = cost;
}
}
Polygon *p = least_cost_poly->get();
//open the neighbours for search
int es = p->edges.size();
for (int i = 0; i < es; i++) {
Polygon::Edge &e = p->edges.write[i];
if (!e.C)
continue;
#ifdef USE_ENTRY_POINT
Vector2 edge[2] = {
_get_vertex(p->edges[i].point),
_get_vertex(p->edges[(i + 1) % es].point)
};
Vector2 edge_entry = Geometry::get_closest_point_to_segment_2d(p->entry, edge);
float distance = p->entry.distance_to(edge_entry) + p->distance;
#else
float distance = p->center.distance_to(e.C->center) + p->distance;
#endif
if (e.C->prev_edge != -1) {
//oh this was visited already, can we win the cost?
if (e.C->distance > distance) {
e.C->prev_edge = e.C_edge;
e.C->distance = distance;
#ifdef USE_ENTRY_POINT
e.C->entry = edge_entry;
#endif
}
} else {
//add to open neighbours
e.C->prev_edge = e.C_edge;
e.C->distance = distance;
#ifdef USE_ENTRY_POINT
e.C->entry = edge_entry;
#endif
open_list.push_back(e.C);
if (e.C == end_poly) {
//oh my reached end! stop algorithm
found_route = true;
break;
}
}
}
if (found_route)
break;
open_list.erase(least_cost_poly);
}
if (found_route) {
Vector<Vector2> path;
if (p_optimize) {
//string pulling
Vector2 apex_point = end_point;
Vector2 portal_left = apex_point;
Vector2 portal_right = apex_point;
Polygon *left_poly = end_poly;
Polygon *right_poly = end_poly;
Polygon *p = end_poly;
while (p) {
Vector2 left;
Vector2 right;
//#define CLOCK_TANGENT(m_a,m_b,m_c) ( ((m_a)-(m_c)).cross((m_a)-(m_b)) )
#define CLOCK_TANGENT(m_a, m_b, m_c) ((((m_a).x - (m_c).x) * ((m_b).y - (m_c).y) - ((m_b).x - (m_c).x) * ((m_a).y - (m_c).y)))
if (p == begin_poly) {
left = begin_point;
right = begin_point;
} else {
int prev = p->prev_edge;
int prev_n = (p->prev_edge + 1) % p->edges.size();
left = _get_vertex(p->edges[prev].point);
right = _get_vertex(p->edges[prev_n].point);
if (p->clockwise) {
SWAP(left, right);
}
/*if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5) < 0){
SWAP(left,right);
}*/
}
bool skip = false;
/*
print_line("-----\nAPEX: "+(apex_point-end_point));
print_line("LEFT:");
print_line("\tPortal: "+(portal_left-end_point));
print_line("\tPoint: "+(left-end_point));
print_line("\tLeft Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_left,left)));
print_line("\tLeft Distance: "+rtos(portal_left.distance_squared_to(apex_point)));
print_line("\tLeft Test: "+rtos(CLOCK_TANGENT(apex_point,left,portal_right)));
print_line("RIGHT:");
print_line("\tPortal: "+(portal_right-end_point));
print_line("\tPoint: "+(right-end_point));
print_line("\tRight Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_right,right)));
print_line("\tRight Distance: "+rtos(portal_right.distance_squared_to(apex_point)));
print_line("\tRight Test: "+rtos(CLOCK_TANGENT(apex_point,right,portal_left)));
*/
if (CLOCK_TANGENT(apex_point, portal_left, left) >= 0) {
//process
if (portal_left.is_equal_approx(apex_point) || CLOCK_TANGENT(apex_point, left, portal_right) > 0) {
left_poly = p;
portal_left = left;
} else {
apex_point = portal_right;
p = right_poly;
left_poly = p;
portal_left = apex_point;
portal_right = apex_point;
if (!path.size() || !path[path.size() - 1].is_equal_approx(apex_point))
path.push_back(apex_point);
skip = true;
}
}
if (!skip && CLOCK_TANGENT(apex_point, portal_right, right) <= 0) {
//process
if (portal_right.is_equal_approx(apex_point) || CLOCK_TANGENT(apex_point, right, portal_left) < 0) {
right_poly = p;
portal_right = right;
} else {
apex_point = portal_left;
p = left_poly;
right_poly = p;
portal_right = apex_point;
portal_left = apex_point;
if (!path.size() || !path[path.size() - 1].is_equal_approx(apex_point))
path.push_back(apex_point);
}
}
if (p != begin_poly)
p = p->edges[p->prev_edge].C;
else
p = NULL;
}
} else {
//midpoints
Polygon *p = end_poly;
while (true) {
int prev = p->prev_edge;
int prev_n = (p->prev_edge + 1) % p->edges.size();
Vector2 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point)) * 0.5;
path.push_back(point);
p = p->edges[prev].C;
if (p == begin_poly)
break;
}
}
if (!path.size() || !path[path.size() - 1].is_equal_approx(begin_point)) {
path.push_back(begin_point); // Add the begin point
} else {
path.write[path.size() - 1] = begin_point; // Replace first midpoint by the exact begin point
}
path.invert();
if (path.size() <= 1 || !path[path.size() - 1].is_equal_approx(end_point)) {
path.push_back(end_point); // Add the end point
} else {
path.write[path.size() - 1] = end_point; // Replace last midpoint by the exact end point
}
return path;
}
return Vector<Vector2>();
}
Vector2 Navigation2D::get_closest_point(const Vector2 &p_point) {
Vector2 closest_point = Vector2();
float closest_point_d = 1e20;
for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
if (!E->get().linked)
continue;
for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
Polygon &p = F->get();
for (int i = 2; i < p.edges.size(); i++) {
if (Geometry::is_point_in_triangle(p_point, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
return p_point; //inside triangle, nothing else to discuss
}
}
}
}
for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
if (!E->get().linked)
continue;
for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
Polygon &p = F->get();
int es = p.edges.size();
for (int i = 0; i < es; i++) {
Vector2 edge[2] = {
_get_vertex(p.edges[i].point),
_get_vertex(p.edges[(i + 1) % es].point)
};
Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_point, edge);
float d = spoint.distance_squared_to(p_point);
if (d < closest_point_d) {
closest_point = spoint;
closest_point_d = d;
}
}
}
}
return closest_point;
}
Object *Navigation2D::get_closest_point_owner(const Vector2 &p_point) {
Object *owner = NULL;
Vector2 closest_point = Vector2();
float closest_point_d = 1e20;
for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
if (!E->get().linked)
continue;
for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
Polygon &p = F->get();
for (int i = 2; i < p.edges.size(); i++) {
if (Geometry::is_point_in_triangle(p_point, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
return E->get().owner;
}
}
}
}
for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
if (!E->get().linked)
continue;
for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
Polygon &p = F->get();
int es = p.edges.size();
for (int i = 0; i < es; i++) {
Vector2 edge[2] = {
_get_vertex(p.edges[i].point),
_get_vertex(p.edges[(i + 1) % es].point)
};
Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_point, edge);
float d = spoint.distance_squared_to(p_point);
if (d < closest_point_d) {
closest_point = spoint;
closest_point_d = d;
owner = E->get().owner;
}
}
}
}
return owner;
}
void Navigation2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("navpoly_add", "mesh", "xform", "owner"), &Navigation2D::navpoly_add, DEFVAL(Variant()));
ClassDB::bind_method(D_METHOD("navpoly_set_transform", "id", "xform"), &Navigation2D::navpoly_set_transform);
ClassDB::bind_method(D_METHOD("navpoly_remove", "id"), &Navigation2D::navpoly_remove);
ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation2D::get_simple_path, DEFVAL(true));
ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Navigation2D::get_closest_point);
ClassDB::bind_method(D_METHOD("get_closest_point_owner", "to_point"), &Navigation2D::get_closest_point_owner);
return Navigation2DServer::get_singleton()->map_get_path(map, p_start, p_end, p_optimize);
}
Navigation2D::Navigation2D() {
ERR_FAIL_COND(sizeof(Point) != 8);
cell_size = 1; // one pixel
last_id = 1;
map = Navigation2DServer::get_singleton()->map_create();
set_cell_size(10); // Ten pixels
set_edge_connection_margin(100);
}

139
scene/2d/navigation_2d.h
View File

@ -38,135 +38,30 @@ class Navigation2D : public Node2D {
GDCLASS(Navigation2D, Node2D);
union Point {
struct {
int64_t x : 32;
int64_t y : 32;
};
uint64_t key;
bool operator<(const Point &p_key) const { return key < p_key.key; }
};
struct EdgeKey {
Point a;
Point 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 Point &p_a = Point(), const Point &p_b = Point()) :
a(p_a),
b(p_b) {
if (a.key > b.key) {
SWAP(a, b);
}
}
};
struct NavMesh;
struct Polygon;
struct ConnectionPending {
Polygon *polygon;
int edge;
};
struct Polygon {
struct Edge {
Point point;
Polygon *C; //connection
int C_edge;
List<ConnectionPending>::Element *P;
Edge() {
C = NULL;
C_edge = -1;
P = NULL;
}
};
Vector<Edge> edges;
Vector2 center;
Vector2 entry;
float distance;
int prev_edge;
bool clockwise;
NavMesh *owner;
};
struct Connection {
Polygon *A;
int A_edge;
Polygon *B;
int B_edge;
List<ConnectionPending> pending;
Connection() {
A = NULL;
B = NULL;
A_edge = -1;
B_edge = -1;
}
};
Map<EdgeKey, Connection> connections;
struct NavMesh {
Object *owner;
Transform2D xform;
bool linked;
Ref<NavigationPolygon> navpoly;
List<Polygon> polygons;
};
_FORCE_INLINE_ Point _get_point(const Vector2 &p_pos) const {
int x = int(Math::floor(p_pos.x / cell_size));
int y = int(Math::floor(p_pos.y / cell_size));
Point p;
p.key = 0;
p.x = x;
p.y = y;
return p;
}
_FORCE_INLINE_ Vector2 _get_vertex(const Point &p_point) const {
return Vector2(p_point.x, p_point.y) * cell_size;
}
void _navpoly_link(int p_id);
void _navpoly_unlink(int p_id);
float cell_size;
Map<int, NavMesh> navpoly_map;
int last_id;
RID map;
real_t cell_size;
real_t edge_connection_margin;
protected:
static void _bind_methods();
void _notification(int p_what);
public:
//API should be as dynamic as possible
int navpoly_add(const Ref<NavigationPolygon> &p_mesh, const Transform2D &p_xform, Object *p_owner = NULL);
void navpoly_set_transform(int p_id, const Transform2D &p_xform);
void navpoly_remove(int p_id);
RID get_rid() const {
return map;
}
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;
}
Vector<Vector2> get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize = true);
Vector2 get_closest_point(const Vector2 &p_point);
Object *get_closest_point_owner(const Vector2 &p_point);
Navigation2D();
};

341
scene/2d/navigation_agent_2d.cpp Normal file
View File

@ -0,0 +1,341 @@
/*************************************************************************/
/* navigation_agent_2d.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 "navigation_agent_2d.h"
#include "core/engine.h"
#include "scene/2d/navigation_2d.h"
#include "servers/navigation_2d_server.h"
void NavigationAgent2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_target_desired_distance", "desired_distance"), &NavigationAgent2D::set_target_desired_distance);
ClassDB::bind_method(D_METHOD("get_target_desired_distance"), &NavigationAgent2D::get_target_desired_distance);
ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationAgent2D::set_radius);
ClassDB::bind_method(D_METHOD("get_radius"), &NavigationAgent2D::get_radius);
ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationAgent2D::set_navigation_node);
ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationAgent2D::get_navigation_node);
ClassDB::bind_method(D_METHOD("set_neighbor_dist", "neighbor_dist"), &NavigationAgent2D::set_neighbor_dist);
ClassDB::bind_method(D_METHOD("get_neighbor_dist"), &NavigationAgent2D::get_neighbor_dist);
ClassDB::bind_method(D_METHOD("set_max_neighbors", "max_neighbors"), &NavigationAgent2D::set_max_neighbors);
ClassDB::bind_method(D_METHOD("get_max_neighbors"), &NavigationAgent2D::get_max_neighbors);
ClassDB::bind_method(D_METHOD("set_time_horizon", "time_horizon"), &NavigationAgent2D::set_time_horizon);
ClassDB::bind_method(D_METHOD("get_time_horizon"), &NavigationAgent2D::get_time_horizon);
ClassDB::bind_method(D_METHOD("set_max_speed", "max_speed"), &NavigationAgent2D::set_max_speed);
ClassDB::bind_method(D_METHOD("get_max_speed"), &NavigationAgent2D::get_max_speed);
ClassDB::bind_method(D_METHOD("set_path_max_distance", "max_speed"), &NavigationAgent2D::set_path_max_distance);
ClassDB::bind_method(D_METHOD("get_path_max_distance"), &NavigationAgent2D::get_path_max_distance);
ClassDB::bind_method(D_METHOD("set_target_location", "location"), &NavigationAgent2D::set_target_location);
ClassDB::bind_method(D_METHOD("get_target_location"), &NavigationAgent2D::get_target_location);
ClassDB::bind_method(D_METHOD("get_next_location"), &NavigationAgent2D::get_next_location);
ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent2D::distance_to_target);
ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationAgent2D::set_velocity);
ClassDB::bind_method(D_METHOD("get_nav_path"), &NavigationAgent2D::get_nav_path);
ClassDB::bind_method(D_METHOD("get_nav_path_index"), &NavigationAgent2D::get_nav_path_index);
ClassDB::bind_method(D_METHOD("is_target_reached"), &NavigationAgent2D::is_target_reached);
ClassDB::bind_method(D_METHOD("is_target_reachable"), &NavigationAgent2D::is_target_reachable);
ClassDB::bind_method(D_METHOD("is_navigation_finished"), &NavigationAgent2D::is_navigation_finished);
ClassDB::bind_method(D_METHOD("get_final_location"), &NavigationAgent2D::get_final_location);
ClassDB::bind_method(D_METHOD("_avoidance_done", "new_velocity"), &NavigationAgent2D::_avoidance_done);
ADD_PROPERTY(PropertyInfo(Variant::REAL, "target_desired_distance", PROPERTY_HINT_RANGE, "0.1,100,0.01"), "set_target_desired_distance", "get_target_desired_distance");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius", PROPERTY_HINT_RANGE, "0.1,500,0.01"), "set_radius", "get_radius");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "neighbor_dist", PROPERTY_HINT_RANGE, "0.1,100000,0.01"), "set_neighbor_dist", "get_neighbor_dist");
ADD_PROPERTY(PropertyInfo(Variant::INT, "max_neighbors", PROPERTY_HINT_RANGE, "1,10000,1"), "set_max_neighbors", "get_max_neighbors");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "time_horizon", PROPERTY_HINT_RANGE, "0.1,10000,0.01"), "set_time_horizon", "get_time_horizon");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "max_speed", PROPERTY_HINT_RANGE, "0.1,100000,0.01"), "set_max_speed", "get_max_speed");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "path_max_distance", PROPERTY_HINT_RANGE, "10,100,1"), "set_path_max_distance", "get_path_max_distance");
ADD_SIGNAL(MethodInfo("path_changed"));
ADD_SIGNAL(MethodInfo("target_reached"));
ADD_SIGNAL(MethodInfo("navigation_finished"));
ADD_SIGNAL(MethodInfo("velocity_computed", PropertyInfo(Variant::VECTOR3, "safe_velocity")));
}
void NavigationAgent2D::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_READY: {
agent_parent = Object::cast_to<Node2D>(get_parent());
Navigation2DServer::get_singleton()->agent_set_callback(agent, this, "_avoidance_done");
// Search the navigation node and set it
{
Navigation2D *nav = NULL;
Node *p = get_parent();
while (p != NULL) {
nav = Object::cast_to<Navigation2D>(p);
if (nav != NULL)
p = NULL;
else
p = p->get_parent();
}
set_navigation(nav);
}
set_physics_process_internal(true);
} break;
case NOTIFICATION_EXIT_TREE: {
agent_parent = NULL;
set_navigation(NULL);
set_physics_process_internal(false);
} break;
case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
if (agent_parent) {
Navigation2DServer::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().get_origin());
if (!target_reached) {
if (distance_to_target() < target_desired_distance) {
emit_signal("target_reached");
target_reached = true;
}
}
}
} break;
}
}
NavigationAgent2D::NavigationAgent2D() :
agent_parent(NULL),
navigation(NULL),
agent(RID()),
target_desired_distance(1.0),
path_max_distance(3.0),
velocity_submitted(false),
target_reached(false),
navigation_finished(true) {
agent = Navigation2DServer::get_singleton()->agent_create();
set_neighbor_dist(500.0);
set_max_neighbors(10);
set_time_horizon(20.0);
set_radius(10.0);
set_max_speed(200.0);
}
NavigationAgent2D::~NavigationAgent2D() {
Navigation2DServer::get_singleton()->free(agent);
agent = RID(); // Pointless
}
void NavigationAgent2D::set_navigation(Navigation2D *p_nav) {
if (navigation == p_nav)
return; // Pointless
navigation = p_nav;
Navigation2DServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
}
void NavigationAgent2D::set_navigation_node(Node *p_nav) {
Navigation2D *nav = Object::cast_to<Navigation2D>(p_nav);
ERR_FAIL_COND(nav == NULL);
set_navigation(nav);
}
Node *NavigationAgent2D::get_navigation_node() const {
return Object::cast_to<Node>(navigation);
}
void NavigationAgent2D::set_target_desired_distance(real_t p_dd) {
target_desired_distance = p_dd;
}
void NavigationAgent2D::set_radius(real_t p_radius) {
radius = p_radius;
Navigation2DServer::get_singleton()->agent_set_radius(agent, radius);
}
void NavigationAgent2D::set_neighbor_dist(real_t p_dist) {
neighbor_dist = p_dist;
Navigation2DServer::get_singleton()->agent_set_neighbor_dist(agent, neighbor_dist);
}
void NavigationAgent2D::set_max_neighbors(int p_count) {
max_neighbors = p_count;
Navigation2DServer::get_singleton()->agent_set_max_neighbors(agent, max_neighbors);
}
void NavigationAgent2D::set_time_horizon(real_t p_time) {
time_horizon = p_time;
Navigation2DServer::get_singleton()->agent_set_time_horizon(agent, time_horizon);
}
void NavigationAgent2D::set_max_speed(real_t p_max_speed) {
max_speed = p_max_speed;
Navigation2DServer::get_singleton()->agent_set_max_speed(agent, max_speed);
}
void NavigationAgent2D::set_path_max_distance(real_t p_pmd) {
path_max_distance = p_pmd;
}
real_t NavigationAgent2D::get_path_max_distance() {
return path_max_distance;
}
void NavigationAgent2D::set_target_location(Vector2 p_location) {
target_location = p_location;
navigation_path.clear();
target_reached = false;
navigation_finished = false;
}
Vector2 NavigationAgent2D::get_target_location() const {
return target_location;
}
Vector2 NavigationAgent2D::get_next_location() {
update_navigation();
if (navigation_path.size() == 0) {
ERR_FAIL_COND_V(agent_parent == NULL, Vector2());
return agent_parent->get_global_transform().get_origin();
} else {
return navigation_path[nav_path_index];
}
}
real_t NavigationAgent2D::distance_to_target() const {
ERR_FAIL_COND_V(agent_parent == NULL, 0.0);
return agent_parent->get_global_transform().get_origin().distance_to(target_location);
}
bool NavigationAgent2D::is_target_reached() const {
return target_reached;
}
bool NavigationAgent2D::is_target_reachable() {
return target_desired_distance >= get_final_location().distance_to(target_location);
}
bool NavigationAgent2D::is_navigation_finished() {
update_navigation();
return navigation_finished;
}
Vector2 NavigationAgent2D::get_final_location() {
update_navigation();
if (navigation_path.size() == 0) {
return Vector2();
}
return navigation_path[navigation_path.size() - 1];
}
void NavigationAgent2D::set_velocity(Vector2 p_velocity) {
target_velocity = p_velocity;
Navigation2DServer::get_singleton()->agent_set_target_velocity(agent, target_velocity);
Navigation2DServer::get_singleton()->agent_set_velocity(agent, prev_safe_velocity);
velocity_submitted = true;
}
void NavigationAgent2D::_avoidance_done(Vector3 p_new_velocity) {
const Vector2 velocity = Vector2(p_new_velocity.x, p_new_velocity.z);
prev_safe_velocity = velocity;
if (!velocity_submitted) {
target_velocity = Vector2();
return;
}
velocity_submitted = false;
emit_signal("velocity_computed", velocity);
}
String NavigationAgent2D::get_configuration_warning() const {
if (!Object::cast_to<Node2D>(get_parent())) {
return TTR("The NavigationAgent2D can be used only under a Node2D node");
}
return String();
}
void NavigationAgent2D::update_navigation() {
if (agent_parent == NULL) return;
if (navigation == NULL) return;
if (update_frame_id == Engine::get_singleton()->get_physics_frames()) return;
update_frame_id = Engine::get_singleton()->get_physics_frames();
Vector2 o = agent_parent->get_global_transform().get_origin();
bool reload_path = false;
if (Navigation2DServer::get_singleton()->agent_is_map_changed(agent)) {
reload_path = true;
} else if (navigation_path.size() == 0) {
reload_path = true;
} else {
// Check if too far from the navigation path
if (nav_path_index > 0) {
Vector2 segment[2];
segment[0] = navigation_path[nav_path_index - 1];
segment[1] = navigation_path[nav_path_index];
Vector2 p = Geometry::get_closest_point_to_segment_2d(o, segment);
if (o.distance_to(p) >= path_max_distance) {
// To faraway, reload path
reload_path = true;
}
}
}
if (reload_path) {
navigation_path = Navigation2DServer::get_singleton()->map_get_path(navigation->get_rid(), o, target_location, true);
navigation_finished = false;
nav_path_index = 0;
emit_signal("path_changed");
}
if (navigation_path.size() == 0)
return;
// Check if we can advance the navigation path
if (navigation_finished == false) {
// Advances to the next far away location.
while (o.distance_to(navigation_path[nav_path_index]) < target_desired_distance) {
nav_path_index += 1;
if (nav_path_index == navigation_path.size()) {
nav_path_index -= 1;
navigation_finished = true;
emit_signal("navigation_finished");
break;
}
}
}
}

150
scene/2d/navigation_agent_2d.h Normal file
View File

@ -0,0 +1,150 @@
/*************************************************************************/
/* navigation_agent_2d.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_AGENT_2D_H
#define NAVIGATION_AGENT_2D_H
#include "core/vector.h"
#include "scene/main/node.h"
class Node2D;
class Navigation2D;
class NavigationAgent2D : public Node {
GDCLASS(NavigationAgent2D, Node);
Node2D *agent_parent;
Navigation2D *navigation;
RID agent;
real_t target_desired_distance;
real_t radius;
real_t neighbor_dist;
int max_neighbors;
real_t time_horizon;
real_t max_speed;
real_t path_max_distance;
Vector2 target_location;
Vector<Vector2> navigation_path;
int nav_path_index;
bool velocity_submitted;
Vector2 prev_safe_velocity;
/// The submitted target velocity
Vector2 target_velocity;
bool target_reached;
bool navigation_finished;
// No initialized on purpose
uint32_t update_frame_id;
protected:
static void _bind_methods();
void _notification(int p_what);
public:
NavigationAgent2D();
virtual ~NavigationAgent2D();
void set_navigation(Navigation2D *p_nav);
const Navigation2D *get_navigation() const {
return navigation;
}
void set_navigation_node(Node *p_nav);
Node *get_navigation_node() const;
RID get_rid() const {
return agent;
}
void set_target_desired_distance(real_t p_dd);
real_t get_target_desired_distance() const {
return target_desired_distance;
}
void set_radius(real_t p_radius);
real_t get_radius() const {
return radius;
}
void set_neighbor_dist(real_t p_dist);
real_t get_neighbor_dist() const {
return neighbor_dist;
}
void set_max_neighbors(int p_count);
int get_max_neighbors() const {
return max_neighbors;
}
void set_time_horizon(real_t p_time);
real_t get_time_horizon() const {
return time_horizon;
}
void set_max_speed(real_t p_max_speed);
real_t get_max_speed() const {
return max_speed;
}
void set_path_max_distance(real_t p_pmd);
real_t get_path_max_distance();
void set_target_location(Vector2 p_location);
Vector2 get_target_location() const;
Vector2 get_next_location();
Vector<Vector2> get_nav_path() const {
return navigation_path;
}
int get_nav_path_index() const {
return nav_path_index;
}
real_t distance_to_target() const;
bool is_target_reached() const;
bool is_target_reachable();
bool is_navigation_finished();
Vector2 get_final_location();
void set_velocity(Vector2 p_velocity);
void _avoidance_done(Vector3 p_new_velocity);
virtual String get_configuration_warning() const;
private:
void update_navigation();
};
#endif

154
scene/2d/navigation_obstacle_2d.cpp Normal file
View File

@ -0,0 +1,154 @@
/*************************************************************************/
/* navigation_obstacle.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 "navigation_obstacle_2d.h"
#include "scene/2d/collision_shape_2d.h"
#include "scene/2d/navigation_2d.h"
#include "scene/2d/physics_body_2d.h"
#include "servers/navigation_2d_server.h"
void NavigationObstacle2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationObstacle2D::set_navigation_node);
ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationObstacle2D::get_navigation_node);
}
void NavigationObstacle2D::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_READY: {
update_agent_shape();
// Search the navigation node and set it
{
Navigation2D *nav = NULL;
Node *p = get_parent();
while (p != NULL) {
nav = Object::cast_to<Navigation2D>(p);
if (nav != NULL)
p = NULL;
else
p = p->get_parent();
}
set_navigation(nav);
}
set_physics_process_internal(true);
} break;
case NOTIFICATION_EXIT_TREE: {
set_navigation(NULL);
set_physics_process_internal(false);
} break;
case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
Node2D *node = Object::cast_to<Node2D>(get_parent());
if (node) {
Navigation2DServer::get_singleton()->agent_set_position(agent, node->get_global_transform().get_origin());
}
} break;
}
}
NavigationObstacle2D::NavigationObstacle2D() :
navigation(NULL),
agent(RID()) {
agent = Navigation2DServer::get_singleton()->agent_create();
}
NavigationObstacle2D::~NavigationObstacle2D() {
Navigation2DServer::get_singleton()->free(agent);
agent = RID(); // Pointless
}
void NavigationObstacle2D::set_navigation(Navigation2D *p_nav) {
if (navigation == p_nav)
return; // Pointless
navigation = p_nav;
Navigation2DServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
}
void NavigationObstacle2D::set_navigation_node(Node *p_nav) {
Navigation2D *nav = Object::cast_to<Navigation2D>(p_nav);
ERR_FAIL_COND(nav == NULL);
set_navigation(nav);
}
Node *NavigationObstacle2D::get_navigation_node() const {
return Object::cast_to<Node>(navigation);
}
String NavigationObstacle2D::get_configuration_warning() const {
if (!Object::cast_to<Node2D>(get_parent())) {
return TTR("The NavigationObstacle2D only serves to provide collision avoidance to a Node2D object.");
}
return String();
}
void NavigationObstacle2D::update_agent_shape() {
Node *node = get_parent();
// Estimate the radius of this physics body
real_t radius = 0.0;
for (int i(0); i < node->get_child_count(); i++) {
// For each collision shape
CollisionShape2D *cs = Object::cast_to<CollisionShape2D>(node->get_child(i));
if (cs) {
// Take the distance between the Body center to the shape center
real_t r = cs->get_transform().get_origin().length();
if (cs->get_shape().is_valid()) {
// and add the enclosing shape radius
r += cs->get_shape()->get_enclosing_radius();
}
Size2 s = cs->get_global_transform().get_scale();
r *= MAX(s.x, s.y);
// Takes the biggest radius
radius = MAX(radius, r);
}
}
Node2D *node_2d = Object::cast_to<Node2D>(node);
if (node_2d) {
Vector2 s = node_2d->get_global_transform().get_scale();
radius *= MAX(s.x, s.y);
}
if (radius == 0.0)
radius = 1.0; // Never a 0 radius
// Initialize the Agent as an object
Navigation2DServer::get_singleton()->agent_set_neighbor_dist(agent, 0.0);
Navigation2DServer::get_singleton()->agent_set_max_neighbors(agent, 0);
Navigation2DServer::get_singleton()->agent_set_time_horizon(agent, 0.0);
Navigation2DServer::get_singleton()->agent_set_radius(agent, radius);
Navigation2DServer::get_singleton()->agent_set_max_speed(agent, 0.0);
}

71
scene/2d/navigation_obstacle_2d.h Normal file
View File

@ -0,0 +1,71 @@
/*************************************************************************/
/* navigation_obstacle.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_OBSTACLE_2D_H
#define NAVIGATION_OBSTACLE_2D_H
#include "scene/main/node.h"
class Navigation2D;
class NavigationObstacle2D : public Node {
GDCLASS(NavigationObstacle2D, Node);
Navigation2D *navigation;
RID agent;
protected:
static void _bind_methods();
void _notification(int p_what);
public:
NavigationObstacle2D();
virtual ~NavigationObstacle2D();
void set_navigation(Navigation2D *p_nav);
const Navigation2D *get_navigation() const {
return navigation;
}
void set_navigation_node(Node *p_nav);
Node *get_navigation_node() const;
RID get_rid() const {
return agent;
}
virtual String get_configuration_warning() const;
private:
void update_agent_shape();
};
#endif

90
scene/2d/navigation_polygon.cpp
View File

@ -32,7 +32,9 @@
#include "core/core_string_names.h"
#include "core/engine.h"
#include "core/os/mutex.h"
#include "navigation_2d.h"
#include "servers/navigation_2d_server.h"
#include "thirdparty/misc/triangulator.h"
@ -80,6 +82,9 @@ bool NavigationPolygon::_edit_is_selected_on_click(const Point2 &p_point, double
void NavigationPolygon::set_vertices(const PoolVector<Vector2> &p_vertices) {
navmesh_generation->lock();
navmesh.unref();
navmesh_generation->unlock();
vertices = p_vertices;
rect_cache_dirty = true;
}
@ -91,6 +96,9 @@ PoolVector<Vector2> NavigationPolygon::get_vertices() const {
void NavigationPolygon::_set_polygons(const Array &p_array) {
navmesh_generation->lock();
navmesh.unref();
navmesh_generation->unlock();
polygons.resize(p_array.size());
for (int i = 0; i < p_array.size(); i++) {
polygons.write[i].indices = p_array[i];
@ -133,6 +141,9 @@ void NavigationPolygon::add_polygon(const Vector<int> &p_polygon) {
Polygon polygon;
polygon.indices = p_polygon;
polygons.push_back(polygon);
navmesh_generation->lock();
navmesh.unref();
navmesh_generation->unlock();
}
void NavigationPolygon::add_outline_at_index(const PoolVector<Vector2> &p_outline, int p_index) {
@ -153,6 +164,34 @@ Vector<int> NavigationPolygon::get_polygon(int p_idx) {
void NavigationPolygon::clear_polygons() {
polygons.clear();
navmesh_generation->lock();
navmesh.unref();
navmesh_generation->unlock();
}
Ref<NavigationMesh> NavigationPolygon::get_mesh() {
navmesh_generation->lock();
if (navmesh.is_null()) {
navmesh.instance();
PoolVector<Vector3> verts;
{
verts.resize(get_vertices().size());
PoolVector<Vector3>::Write w = verts.write();
PoolVector<Vector2>::Read r = get_vertices().read();
for (int i(0); i < get_vertices().size(); i++) {
w[i] = Vector3(r[i].x, 0.0, r[i].y);
}
}
navmesh->set_vertices(verts);
for (int i(0); i < get_polygon_count(); i++) {
navmesh->add_polygon(get_polygon(i));
}
}
navmesh_generation->unlock();
return navmesh;
}
void NavigationPolygon::add_outline(const PoolVector<Vector2> &p_outline) {
@ -191,6 +230,9 @@ void NavigationPolygon::clear_outlines() {
}
void NavigationPolygon::make_polygons_from_outlines() {
navmesh_generation->lock();
navmesh.unref();
navmesh_generation->unlock();
List<TriangulatorPoly> in_poly, out_poly;
Vector2 outside_point(-1e10, -1e10);
@ -320,7 +362,9 @@ void NavigationPolygon::_bind_methods() {
}
NavigationPolygon::NavigationPolygon() :
rect_cache_dirty(true) {
rect_cache_dirty(true),
navmesh_generation(NULL) {
navmesh_generation = Mutex::create();
}
void NavigationPolygonInstance::set_enabled(bool p_enabled) {
@ -334,18 +378,12 @@ void NavigationPolygonInstance::set_enabled(bool p_enabled) {
if (!enabled) {
if (nav_id != -1) {
navigation->navpoly_remove(nav_id);
nav_id = -1;
}
Navigation2DServer::get_singleton()->region_set_map(region, RID());
} else {
if (navigation) {
if (navpoly.is_valid()) {
nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
}
Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
}
}
@ -382,9 +420,9 @@ void NavigationPolygonInstance::_notification(int p_what) {
navigation = Object::cast_to<Navigation2D>(c);
if (navigation) {
if (enabled && navpoly.is_valid()) {
if (enabled) {
nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
}
break;
}
@ -395,19 +433,14 @@ void NavigationPolygonInstance::_notification(int p_what) {
} break;
case NOTIFICATION_TRANSFORM_CHANGED: {
if (navigation && nav_id != -1) {
navigation->navpoly_set_transform(nav_id, get_relative_transform_to_parent(navigation));
}
Navigation2DServer::get_singleton()->region_set_transform(region, get_global_transform());
} break;
case NOTIFICATION_EXIT_TREE: {
if (navigation) {
if (nav_id != -1) {
navigation->navpoly_remove(nav_id);
nav_id = -1;
}
Navigation2DServer::get_singleton()->region_set_map(region, RID());
}
navigation = NULL;
} break;
@ -466,24 +499,18 @@ void NavigationPolygonInstance::set_navigation_polygon(const Ref<NavigationPolyg
return;
}
if (navigation && nav_id != -1) {
navigation->navpoly_remove(nav_id);
nav_id = -1;
}
if (navpoly.is_valid()) {
navpoly->disconnect(CoreStringNames::get_singleton()->changed, this, "_navpoly_changed");
}
navpoly = p_navpoly;
Navigation2DServer::get_singleton()->region_set_navpoly(region, p_navpoly);
if (navpoly.is_valid()) {
navpoly->connect(CoreStringNames::get_singleton()->changed, this, "_navpoly_changed");
}
_navpoly_changed();
if (navigation && navpoly.is_valid() && enabled) {
nav_id = navigation->navpoly_add(navpoly, get_relative_transform_to_parent(navigation), this);
}
_change_notify("navpoly");
update_configuration_warning();
}
@ -536,8 +563,13 @@ void NavigationPolygonInstance::_bind_methods() {
NavigationPolygonInstance::NavigationPolygonInstance() {
navigation = NULL;
nav_id = -1;
enabled = true;
set_notify_transform(true);
region = Navigation2DServer::get_singleton()->region_create();
navigation = NULL;
}
NavigationPolygonInstance::~NavigationPolygonInstance() {
Navigation2DServer::get_singleton()->free(region);
}

12
scene/2d/navigation_polygon.h
View File

@ -32,6 +32,9 @@
#define NAVIGATION_POLYGON_H
#include "scene/2d/node_2d.h"
#include "scene/resources/navigation_mesh.h"
class Mutex;
class NavigationPolygon : public Resource {
@ -47,6 +50,10 @@ class NavigationPolygon : public Resource {
mutable Rect2 item_rect;
mutable bool rect_cache_dirty;
Mutex *navmesh_generation;
// Navigation mesh
Ref<NavigationMesh> navmesh;
protected:
static void _bind_methods();
@ -81,6 +88,8 @@ public:
Vector<int> get_polygon(int p_idx);
void clear_polygons();
Ref<NavigationMesh> get_mesh();
NavigationPolygon();
};
@ -91,7 +100,7 @@ class NavigationPolygonInstance : public Node2D {
GDCLASS(NavigationPolygonInstance, Node2D);
bool enabled;
int nav_id;
RID region;
Navigation2D *navigation;
Ref<NavigationPolygon> navpoly;
@ -116,6 +125,7 @@ public:
String get_configuration_warning() const;
NavigationPolygonInstance();
~NavigationPolygonInstance();
};
#endif // NAVIGATIONPOLYGON_H

16
scene/2d/tile_map.cpp
View File

@ -35,6 +35,7 @@
#include "core/method_bind_ext.gen.inc"
#include "core/os/os.h"
#include "scene/2d/area_2d.h"
#include "servers/navigation_2d_server.h"
#include "servers/physics_2d_server.h"
int TileMap::_get_quadrant_size() const {
@ -86,7 +87,7 @@ void TileMap::_notification(int p_what) {
if (navigation) {
for (Map<PosKey, Quadrant::NavPoly>::Element *F = q.navpoly_ids.front(); F; F = F->next()) {
navigation->navpoly_remove(F->get().id);
Navigation2DServer::get_singleton()->region_set_map(F->get().region, RID());
}
q.navpoly_ids.clear();
}
@ -163,7 +164,7 @@ void TileMap::_update_quadrant_transform() {
if (navigation) {
for (Map<PosKey, Quadrant::NavPoly>::Element *F = q.navpoly_ids.front(); F; F = F->next()) {
navigation->navpoly_set_transform(F->get().id, nav_rel * F->get().xform);
Navigation2DServer::get_singleton()->region_set_transform(F->get().region, nav_rel * F->get().xform);
}
}
@ -377,7 +378,7 @@ void TileMap::update_dirty_quadrants() {
if (navigation) {
for (Map<PosKey, Quadrant::NavPoly>::Element *E = q.navpoly_ids.front(); E; E = E->next()) {
navigation->navpoly_remove(E->get().id);
Navigation2DServer::get_singleton()->region_set_map(E->get().region, RID());
}
q.navpoly_ids.clear();
}
@ -611,10 +612,13 @@ void TileMap::update_dirty_quadrants() {
xform.set_origin(offset.floor() + q.pos);
_fix_cell_transform(xform, c, npoly_ofs, s);
int pid = navigation->navpoly_add(navpoly, nav_rel * xform);
RID region = Navigation2DServer::get_singleton()->region_create();
Navigation2DServer::get_singleton()->region_set_map(region, navigation->get_rid());
Navigation2DServer::get_singleton()->region_set_transform(region, nav_rel * xform);
Navigation2DServer::get_singleton()->region_set_navpoly(region, navpoly);
Quadrant::NavPoly np;
np.id = pid;
np.region = region;
np.xform = xform;
q.navpoly_ids[E->key()] = np;
@ -809,7 +813,7 @@ void TileMap::_erase_quadrant(Map<PosKey, Quadrant>::Element *Q) {
if (navigation) {
for (Map<PosKey, Quadrant::NavPoly>::Element *E = q.navpoly_ids.front(); E; E = E->next()) {
navigation->navpoly_remove(E->get().id);
Navigation2DServer::get_singleton()->region_set_map(E->get().region, RID());
}
q.navpoly_ids.clear();
}

2
scene/2d/tile_map.h
View File

@ -139,7 +139,7 @@ private:
SelfList<Quadrant> dirty_list;
struct NavPoly {
int id;
RID region;
Transform2D xform;
};

708
scene/3d/navigation.cpp
View File

@ -30,669 +30,17 @@
#include "navigation.h"
#define USE_ENTRY_POINT
void Navigation::_navmesh_link(int p_id) {
ERR_FAIL_COND(!navmesh_map.has(p_id));
NavMesh &nm = navmesh_map[p_id];
ERR_FAIL_COND(nm.linked);
ERR_FAIL_COND(nm.navmesh.is_null());
PoolVector<Vector3> vertices = nm.navmesh->get_vertices();
int len = vertices.size();
if (len == 0)
return;
PoolVector<Vector3>::Read r = vertices.read();
for (int i = 0; i < nm.navmesh->get_polygon_count(); i++) {
//build
List<Polygon>::Element *P = nm.polygons.push_back(Polygon());
Polygon &p = P->get();
p.owner = &nm;
Vector<int> poly = nm.navmesh->get_polygon(i);
int plen = poly.size();
const int *indices = poly.ptr();
bool valid = true;
p.edges.resize(plen);
Vector3 center;
float sum = 0;
for (int j = 0; j < plen; j++) {
int idx = indices[j];
if (idx < 0 || idx >= len) {
valid = false;
break;
}
Polygon::Edge e;
Vector3 ep = nm.xform.xform(r[idx]);
center += ep;
e.point = _get_point(ep);
p.edges.write[j] = e;
if (j >= 2) {
Vector3 epa = nm.xform.xform(r[indices[j - 2]]);
Vector3 epb = nm.xform.xform(r[indices[j - 1]]);
sum += up.dot((epb - epa).cross(ep - epa));
}
}
p.clockwise = sum > 0;
if (!valid) {
nm.polygons.pop_back();
ERR_CONTINUE(!valid);
}
p.center = center;
if (plen != 0) {
p.center /= plen;
}
//connect
for (int j = 0; j < plen; j++) {
int next = (j + 1) % plen;
EdgeKey ek(p.edges[j].point, p.edges[next].point);
Map<EdgeKey, Connection>::Element *C = connections.find(ek);
if (!C) {
Connection c;
c.A = &p;
c.A_edge = j;
c.B = NULL;
c.B_edge = -1;
connections[ek] = c;
} else {
if (C->get().B != NULL) {
ConnectionPending pending;
pending.polygon = &p;
pending.edge = j;
p.edges.write[j].P = C->get().pending.push_back(pending);
continue;
}
C->get().B = &p;
C->get().B_edge = j;
C->get().A->edges.write[C->get().A_edge].C = &p;
C->get().A->edges.write[C->get().A_edge].C_edge = j;
p.edges.write[j].C = C->get().A;
p.edges.write[j].C_edge = C->get().A_edge;
//connection successful.
}
}
}
nm.linked = true;
}
void Navigation::_navmesh_unlink(int p_id) {
ERR_FAIL_COND(!navmesh_map.has(p_id));
NavMesh &nm = navmesh_map[p_id];
ERR_FAIL_COND(!nm.linked);
for (List<Polygon>::Element *E = nm.polygons.front(); E; E = E->next()) {
Polygon &p = E->get();
int ec = p.edges.size();
Polygon::Edge *edges = p.edges.ptrw();
for (int i = 0; i < ec; i++) {
int next = (i + 1) % ec;
EdgeKey ek(edges[i].point, edges[next].point);
Map<EdgeKey, Connection>::Element *C = connections.find(ek);
ERR_CONTINUE(!C);
if (edges[i].P) {
C->get().pending.erase(edges[i].P);
edges[i].P = NULL;
} else if (C->get().B) {
//disconnect
C->get().B->edges.write[C->get().B_edge].C = NULL;
C->get().B->edges.write[C->get().B_edge].C_edge = -1;
C->get().A->edges.write[C->get().A_edge].C = NULL;
C->get().A->edges.write[C->get().A_edge].C_edge = -1;
if (C->get().A == &E->get()) {
C->get().A = C->get().B;
C->get().A_edge = C->get().B_edge;
}
C->get().B = NULL;
C->get().B_edge = -1;
if (C->get().pending.size()) {
//reconnect if something is pending
ConnectionPending cp = C->get().pending.front()->get();
C->get().pending.pop_front();
C->get().B = cp.polygon;
C->get().B_edge = cp.edge;
C->get().A->edges.write[C->get().A_edge].C = cp.polygon;
C->get().A->edges.write[C->get().A_edge].C_edge = cp.edge;
cp.polygon->edges.write[cp.edge].C = C->get().A;
cp.polygon->edges.write[cp.edge].C_edge = C->get().A_edge;
cp.polygon->edges.write[cp.edge].P = NULL;
}
} else {
connections.erase(C);
//erase
}
}
}
nm.polygons.clear();
nm.linked = false;
}
int Navigation::navmesh_add(const Ref<NavigationMesh> &p_mesh, const Transform &p_xform, Object *p_owner) {
int id = last_id++;
NavMesh nm;
nm.linked = false;
nm.navmesh = p_mesh;
nm.xform = p_xform;
nm.owner = p_owner;
navmesh_map[id] = nm;
_navmesh_link(id);
return id;
}
void Navigation::navmesh_set_transform(int p_id, const Transform &p_xform) {
ERR_FAIL_COND(!navmesh_map.has(p_id));
NavMesh &nm = navmesh_map[p_id];
if (nm.xform == p_xform)
return; //bleh
_navmesh_unlink(p_id);
nm.xform = p_xform;
_navmesh_link(p_id);
}
void Navigation::navmesh_remove(int p_id) {
ERR_FAIL_COND_MSG(!navmesh_map.has(p_id), "Trying to remove nonexisting navmesh with id: " + itos(p_id));
_navmesh_unlink(p_id);
navmesh_map.erase(p_id);
}
void Navigation::_clip_path(Vector<Vector3> &path, Polygon *from_poly, const Vector3 &p_to_point, Polygon *p_to_poly) {
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 pe = from_poly->prev_edge;
Vector3 a = _get_vertex(from_poly->edges[pe].point);
Vector3 b = _get_vertex(from_poly->edges[(pe + 1) % from_poly->edges.size()].point);
from_poly = from_poly->edges[pe].C;
ERR_FAIL_COND(!from_poly);
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);
}
}
}
}
}
#include "servers/navigation_server.h"
Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize) {
Polygon *begin_poly = NULL;
Polygon *end_poly = NULL;
Vector3 begin_point;
Vector3 end_point;
float begin_d = 1e20;
float end_d = 1e20;
for (Map<int, NavMesh>::Element *E = navmesh_map.front(); E; E = E->next()) {
if (!E->get().linked)
continue;
for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
Polygon &p = F->get();
for (int i = 2; i < p.edges.size(); i++) {
Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
Vector3 spoint = f.get_closest_point_to(p_start);
float dpoint = spoint.distance_to(p_start);
if (dpoint < begin_d) {
begin_d = dpoint;
begin_poly = &p;
begin_point = spoint;
}
spoint = f.get_closest_point_to(p_end);
dpoint = spoint.distance_to(p_end);
if (dpoint < end_d) {
end_d = dpoint;
end_poly = &p;
end_point = spoint;
}
}
p.prev_edge = -1;
}
}
if (!begin_poly || !end_poly) {
return Vector<Vector3>(); //no path
}
if (begin_poly == end_poly) {
Vector<Vector3> path;
path.resize(2);
path.write[0] = begin_point;
path.write[1] = end_point;
return path;
}
bool found_route = false;
List<Polygon *> open_list;
for (int i = 0; i < begin_poly->edges.size(); i++) {
if (begin_poly->edges[i].C) {
begin_poly->edges[i].C->prev_edge = begin_poly->edges[i].C_edge;
#ifdef USE_ENTRY_POINT
Vector3 edge[2] = {
_get_vertex(begin_poly->edges[i].point),
_get_vertex(begin_poly->edges[(i + 1) % begin_poly->edges.size()].point)
};
Vector3 entry = Geometry::get_closest_point_to_segment(begin_poly->entry, edge);
begin_poly->edges[i].C->distance = begin_point.distance_to(entry);
begin_poly->edges[i].C->entry = entry;
#else
begin_poly->edges[i].C->distance = begin_poly->center.distance_to(begin_poly->edges[i].C->center);
#endif
open_list.push_back(begin_poly->edges[i].C);
}
}
while (!found_route) {
if (open_list.size() == 0) {
break;
}
//check open list
List<Polygon *>::Element *least_cost_poly = NULL;
float least_cost = 1e30;
//this could be faster (cache previous results)
for (List<Polygon *>::Element *E = open_list.front(); E; E = E->next()) {
Polygon *p = E->get();
float cost = p->distance;
#ifdef USE_ENTRY_POINT
cost += p->entry.distance_to(end_point);
#else
cost += p->center.distance_to(end_point);
#endif
if (cost < least_cost) {
least_cost_poly = E;
least_cost = cost;
}
}
Polygon *p = least_cost_poly->get();
//open the neighbours for search
if (p == end_poly) {
//oh my reached end! stop algorithm
found_route = true;
break;
}
for (int i = 0; i < p->edges.size(); i++) {
Polygon::Edge &e = p->edges.write[i];
if (!e.C)
continue;
#ifdef USE_ENTRY_POINT
Vector3 edge[2] = {
_get_vertex(p->edges[i].point),
_get_vertex(p->edges[(i + 1) % p->edges.size()].point)
};
Vector3 entry = Geometry::get_closest_point_to_segment(p->entry, edge);
float distance = p->entry.distance_to(entry) + p->distance;
#else
float distance = p->center.distance_to(e.C->center) + p->distance;
#endif
if (e.C->prev_edge != -1) {
//oh this was visited already, can we win the cost?
if (e.C->distance > distance) {
e.C->prev_edge = e.C_edge;
e.C->distance = distance;
#ifdef USE_ENTRY_POINT
e.C->entry = entry;
#endif
}
} else {
//add to open neighbours
e.C->prev_edge = e.C_edge;
e.C->distance = distance;
#ifdef USE_ENTRY_POINT
e.C->entry = entry;
#endif
open_list.push_back(e.C);
}
}
open_list.erase(least_cost_poly);
}
if (found_route) {
Vector<Vector3> path;
if (p_optimize) {
//string pulling
Polygon *apex_poly = end_poly;
Vector3 apex_point = end_point;
Vector3 portal_left = apex_point;
Vector3 portal_right = apex_point;
Polygon *left_poly = end_poly;
Polygon *right_poly = end_poly;
Polygon *p = end_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 == begin_poly) {
left = begin_point;
right = begin_point;
} else {
int prev = p->prev_edge;
int prev_n = (p->prev_edge + 1) % p->edges.size();
left = _get_vertex(p->edges[prev].point);
right = _get_vertex(p->edges[prev_n].point);
//if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5).dot(up) < 0){
if (p->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(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(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 != begin_poly)
p = p->edges[p->prev_edge].C;
else
p = NULL;
}
if (path[path.size() - 1] != begin_point)
path.push_back(begin_point);
path.invert();
} else {
//midpoints
Polygon *p = end_poly;
path.push_back(end_point);
while (true) {
int prev = p->prev_edge;
#ifdef USE_ENTRY_POINT
Vector3 point = p->entry;
#else
int prev_n = (p->prev_edge + 1) % p->edges.size();
Vector3 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point)) * 0.5;
#endif
path.push_back(point);
p = p->edges[prev].C;
if (p == begin_poly)
break;
}
path.push_back(begin_point);
path.invert();
}
return path;
}
return Vector<Vector3>();
}
Vector3 Navigation::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool &p_use_collision) {
bool use_collision = p_use_collision;
Vector3 closest_point;
float closest_point_d = 1e20;
for (Map<int, NavMesh>::Element *E = navmesh_map.front(); E; E = E->next()) {
if (!E->get().linked)
continue;
for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
Polygon &p = F->get();
for (int i = 2; i < p.edges.size(); i++) {
Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
Vector3 inters;
if (f.intersects_segment(p_from, p_to, &inters)) {
if (!use_collision) {
closest_point = inters;
use_collision = true;
closest_point_d = p_from.distance_to(inters);
} else if (closest_point_d > inters.distance_to(p_from)) {
closest_point = inters;
closest_point_d = p_from.distance_to(inters);
}
}
}
if (!use_collision) {
for (int i = 0; i < p.edges.size(); i++) {
Vector3 a, b;
Geometry::get_closest_points_between_segments(p_from, p_to, _get_vertex(p.edges[i].point), _get_vertex(p.edges[(i + 1) % p.edges.size()].point), a, b);
float d = a.distance_to(b);
if (d < closest_point_d) {
closest_point_d = d;
closest_point = b;
}
}
}
}
}
return closest_point;
}
Vector3 Navigation::get_closest_point(const Vector3 &p_point) {
Vector3 closest_point;
float closest_point_d = 1e20;
for (Map<int, NavMesh>::Element *E = navmesh_map.front(); E; E = E->next()) {
if (!E->get().linked)
continue;
for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
Polygon &p = F->get();
for (int i = 2; i < p.edges.size(); i++) {
Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
Vector3 inters = f.get_closest_point_to(p_point);
float d = inters.distance_to(p_point);
if (d < closest_point_d) {
closest_point = inters;
closest_point_d = d;
}
}
}
}
return closest_point;
}
Vector3 Navigation::get_closest_point_normal(const Vector3 &p_point) {
Vector3 closest_point;
Vector3 closest_normal;
float closest_point_d = 1e20;
for (Map<int, NavMesh>::Element *E = navmesh_map.front(); E; E = E->next()) {
if (!E->get().linked)
continue;
for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
Polygon &p = F->get();
for (int i = 2; i < p.edges.size(); i++) {
Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
Vector3 inters = f.get_closest_point_to(p_point);
float d = inters.distance_to(p_point);
if (d < closest_point_d) {
closest_point = inters;
closest_point_d = d;
closest_normal = f.get_plane().normal;
}
}
}
}
return closest_normal;
}
Object *Navigation::get_closest_point_owner(const Vector3 &p_point) {
Vector3 closest_point;
Object *owner = NULL;
float closest_point_d = 1e20;
for (Map<int, NavMesh>::Element *E = navmesh_map.front(); E; E = E->next()) {
if (!E->get().linked)
continue;
for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
Polygon &p = F->get();
for (int i = 2; i < p.edges.size(); i++) {
Face3 f(_get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point));
Vector3 inters = f.get_closest_point_to(p_point);
float d = inters.distance_to(p_point);
if (d < closest_point_d) {
closest_point = inters;
closest_point_d = d;
owner = E->get().owner;
}
}
}
}
return owner;
return NavigationServer::get_singleton()->map_get_path(map, p_start, p_end, p_optimize);
}
void Navigation::set_up_vector(const Vector3 &p_up) {
up = p_up;
NavigationServer::get_singleton()->map_set_up(map, up);
}
Vector3 Navigation::get_up_vector() const {
@ -700,28 +48,58 @@ Vector3 Navigation::get_up_vector() const {
return up;
}
void Navigation::set_cell_size(float p_cell_size) {
cell_size = p_cell_size;
NavigationServer::get_singleton()->map_set_cell_size(map, cell_size);
}
void Navigation::set_edge_connection_margin(float p_edge_connection_margin) {
edge_connection_margin = p_edge_connection_margin;
NavigationServer::get_singleton()->map_set_edge_connection_margin(map, edge_connection_margin);
}
void Navigation::_bind_methods() {
ClassDB::bind_method(D_METHOD("navmesh_add", "mesh", "xform", "owner"), &Navigation::navmesh_add, DEFVAL(Variant()));
ClassDB::bind_method(D_METHOD("navmesh_set_transform", "id", "xform"), &Navigation::navmesh_set_transform);
ClassDB::bind_method(D_METHOD("navmesh_remove", "id"), &Navigation::navmesh_remove);
ClassDB::bind_method(D_METHOD("get_rid"), &Navigation::get_rid);
ClassDB::bind_method(D_METHOD("get_simple_path", "start", "end", "optimize"), &Navigation::get_simple_path, DEFVAL(true));
ClassDB::bind_method(D_METHOD("get_closest_point_to_segment", "start", "end", "use_collision"), &Navigation::get_closest_point_to_segment, DEFVAL(false));
ClassDB::bind_method(D_METHOD("get_closest_point", "to_point"), &Navigation::get_closest_point);
ClassDB::bind_method(D_METHOD("get_closest_point_normal", "to_point"), &Navigation::get_closest_point_normal);
ClassDB::bind_method(D_METHOD("get_closest_point_owner", "to_point"), &Navigation::get_closest_point_owner);
ClassDB::bind_method(D_METHOD("set_up_vector", "up"), &Navigation::set_up_vector);
ClassDB::bind_method(D_METHOD("get_up_vector"), &Navigation::get_up_vector);
ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &Navigation::set_cell_size);
ClassDB::bind_method(D_METHOD("get_cell_size"), &Navigation::get_cell_size);
ClassDB::bind_method(D_METHOD("set_edge_connection_margin", "margin"), &Navigation::set_edge_connection_margin);
ClassDB::bind_method(D_METHOD("get_edge_connection_margin"), &Navigation::get_edge_connection_margin);
ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "up_vector"), "set_up_vector", "get_up_vector");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "cell_size"), "set_cell_size", "get_cell_size");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "edge_connection_margin"), "set_edge_connection_margin", "get_edge_connection_margin");
}
void Navigation::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_READY: {
NavigationServer::get_singleton()->map_set_active(map, true);
} break;
case NOTIFICATION_EXIT_TREE: {
NavigationServer::get_singleton()->map_set_active(map, false);
} break;
}
}
Navigation::Navigation() {
ERR_FAIL_COND(sizeof(Point) != 8);
cell_size = 0.01; //one centimeter
last_id = 1;
map = NavigationServer::get_singleton()->map_create();
set_cell_size(0.3);
set_edge_connection_margin(5.0); // Five meters, depends alot on the agents radius
up = Vector3(0, 1, 0);
}
Navigation::~Navigation() {
NavigationServer::get_singleton()->free(map);
}

147
scene/3d/navigation.h
View File

@ -31,154 +31,45 @@
#ifndef NAVIGATION_H
#define NAVIGATION_H
#include "scene/3d/navigation_mesh.h"
#include "scene/3d/navigation_mesh_instance.h"
#include "scene/3d/spatial.h"
class Navigation : public Spatial {
GDCLASS(Navigation, Spatial);
union Point {
struct {
int64_t x : 21;
int64_t y : 22;
int64_t z : 21;
};
uint64_t key;
bool operator<(const Point &p_key) const { return key < p_key.key; }
};
struct EdgeKey {
Point a;
Point 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 Point &p_a = Point(), const Point &p_b = Point()) :
a(p_a),
b(p_b) {
if (a.key > b.key) {
SWAP(a, b);
}
}
};
struct NavMesh;
struct Polygon;
struct ConnectionPending {
Polygon *polygon;
int edge;
};
struct Polygon {
struct Edge {
Point point;
Polygon *C; //connection
int C_edge;
List<ConnectionPending>::Element *P;
Edge() {
C = NULL;
C_edge = -1;
P = NULL;
}
};
Vector<Edge> edges;
Vector3 center;
Vector3 entry;
float distance;
int prev_edge;
bool clockwise;
NavMesh *owner;
};
struct Connection {
Polygon *A;
int A_edge;
Polygon *B;
int B_edge;
List<ConnectionPending> pending;
Connection() {
A = NULL;
B = NULL;
A_edge = -1;
B_edge = -1;
}
};
Map<EdgeKey, Connection> connections;
struct NavMesh {
Object *owner;
Transform xform;
bool linked;
Ref<NavigationMesh> navmesh;
List<Polygon> polygons;
};
_FORCE_INLINE_ Point _get_point(const Vector3 &p_pos) const {
int x = int(Math::floor(p_pos.x / cell_size));
int y = int(Math::floor(p_pos.y / cell_size));
int z = int(Math::floor(p_pos.z / cell_size));
Point p;
p.key = 0;
p.x = x;
p.y = y;
p.z = z;
return p;
}
_FORCE_INLINE_ Vector3 _get_vertex(const Point &p_point) const {
return Vector3(p_point.x, p_point.y, p_point.z) * cell_size;
}
void _navmesh_link(int p_id);
void _navmesh_unlink(int p_id);
float cell_size;
Map<int, NavMesh> navmesh_map;
int last_id;
RID map;
Vector3 up;
void _clip_path(Vector<Vector3> &path, Polygon *from_poly, const Vector3 &p_to_point, Polygon *p_to_poly);
real_t cell_size;
real_t edge_connection_margin;
protected:
static void _bind_methods();
void _notification(int p_what);
public:
RID get_rid() const {
return map;
}
void set_up_vector(const Vector3 &p_up);
Vector3 get_up_vector() const;
//API should be as dynamic as possible
int navmesh_add(const Ref<NavigationMesh> &p_mesh, const Transform &p_xform, Object *p_owner = NULL);
void navmesh_set_transform(int p_id, const Transform &p_xform);
void navmesh_remove(int p_id);
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;
}
Vector<Vector3> get_simple_path(const Vector3 &p_start, const Vector3 &p_end, bool p_optimize = true);
Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool &p_use_collision = false);
Vector3 get_closest_point(const Vector3 &p_point);
Vector3 get_closest_point_normal(const Vector3 &p_point);
Object *get_closest_point_owner(const Vector3 &p_point);
Navigation();
~Navigation();
};
#endif // NAVIGATION_H

361
scene/3d/navigation_agent.cpp Normal file
View File

@ -0,0 +1,361 @@
/*************************************************************************/
/* navigation_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 "navigation_agent.h"
#include "core/engine.h"
#include "scene/3d/navigation.h"
#include "servers/navigation_server.h"
void NavigationAgent::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_target_desired_distance", "desired_distance"), &NavigationAgent::set_target_desired_distance);
ClassDB::bind_method(D_METHOD("get_target_desired_distance"), &NavigationAgent::get_target_desired_distance);
ClassDB::bind_method(D_METHOD("set_radius", "radius"), &NavigationAgent::set_radius);
ClassDB::bind_method(D_METHOD("get_radius"), &NavigationAgent::get_radius);
ClassDB::bind_method(D_METHOD("set_player_center_height", "player_center_height"), &NavigationAgent::set_player_center_height);
ClassDB::bind_method(D_METHOD("get_player_center_height"), &NavigationAgent::get_player_center_height);
ClassDB::bind_method(D_METHOD("set_ignore_y", "ignore"), &NavigationAgent::set_ignore_y);
ClassDB::bind_method(D_METHOD("get_ignore_y"), &NavigationAgent::get_ignore_y);
ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationAgent::set_navigation_node);
ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationAgent::get_navigation_node);
ClassDB::bind_method(D_METHOD("set_neighbor_dist", "neighbor_dist"), &NavigationAgent::set_neighbor_dist);
ClassDB::bind_method(D_METHOD("get_neighbor_dist"), &NavigationAgent::get_neighbor_dist);
ClassDB::bind_method(D_METHOD("set_max_neighbors", "max_neighbors"), &NavigationAgent::set_max_neighbors);
ClassDB::bind_method(D_METHOD("get_max_neighbors"), &NavigationAgent::get_max_neighbors);
ClassDB::bind_method(D_METHOD("set_time_horizon", "time_horizon"), &NavigationAgent::set_time_horizon);
ClassDB::bind_method(D_METHOD("get_time_horizon"), &NavigationAgent::get_time_horizon);
ClassDB::bind_method(D_METHOD("set_max_speed", "max_speed"), &NavigationAgent::set_max_speed);
ClassDB::bind_method(D_METHOD("get_max_speed"), &NavigationAgent::get_max_speed);
ClassDB::bind_method(D_METHOD("set_path_max_distance", "max_speed"), &NavigationAgent::set_path_max_distance);
ClassDB::bind_method(D_METHOD("get_path_max_distance"), &NavigationAgent::get_path_max_distance);
ClassDB::bind_method(D_METHOD("set_target_location", "location"), &NavigationAgent::set_target_location);
ClassDB::bind_method(D_METHOD("get_target_location"), &NavigationAgent::get_target_location);
ClassDB::bind_method(D_METHOD("get_next_location"), &NavigationAgent::get_next_location);
ClassDB::bind_method(D_METHOD("distance_to_target"), &NavigationAgent::distance_to_target);
ClassDB::bind_method(D_METHOD("set_velocity", "velocity"), &NavigationAgent::set_velocity);
ClassDB::bind_method(D_METHOD("get_nav_path"), &NavigationAgent::get_nav_path);
ClassDB::bind_method(D_METHOD("get_nav_path_index"), &NavigationAgent::get_nav_path_index);
ClassDB::bind_method(D_METHOD("is_target_reached"), &NavigationAgent::is_target_reached);
ClassDB::bind_method(D_METHOD("is_target_reachable"), &NavigationAgent::is_target_reachable);
ClassDB::bind_method(D_METHOD("is_navigation_finished"), &NavigationAgent::is_navigation_finished);
ClassDB::bind_method(D_METHOD("get_final_location"), &NavigationAgent::get_final_location);
ClassDB::bind_method(D_METHOD("_avoidance_done", "new_velocity"), &NavigationAgent::_avoidance_done);
ADD_PROPERTY(PropertyInfo(Variant::REAL, "target_desired_distance", PROPERTY_HINT_RANGE, "0.1,100,0.01"), "set_target_desired_distance", "get_target_desired_distance");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "radius", PROPERTY_HINT_RANGE, "0.1,100,0.01"), "set_radius", "get_radius");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "player_center_height", PROPERTY_HINT_RANGE, "-100.0,100,0.01"), "set_player_center_height", "get_player_center_height");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "neighbor_dist", PROPERTY_HINT_RANGE, "0.1,10000,0.01"), "set_neighbor_dist", "get_neighbor_dist");
ADD_PROPERTY(PropertyInfo(Variant::INT, "max_neighbors", PROPERTY_HINT_RANGE, "1,10000,1"), "set_max_neighbors", "get_max_neighbors");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "time_horizon", PROPERTY_HINT_RANGE, "0.01,100,0.01"), "set_time_horizon", "get_time_horizon");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "max_speed", PROPERTY_HINT_RANGE, "0.1,10000,0.01"), "set_max_speed", "get_max_speed");
ADD_PROPERTY(PropertyInfo(Variant::REAL, "path_max_distance", PROPERTY_HINT_RANGE, "0.01,100,0.1"), "set_path_max_distance", "get_path_max_distance");
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "ignore_y"), "set_ignore_y", "get_ignore_y");
ADD_SIGNAL(MethodInfo("path_changed"));
ADD_SIGNAL(MethodInfo("target_reached"));
ADD_SIGNAL(MethodInfo("navigation_finished"));
ADD_SIGNAL(MethodInfo("velocity_computed", PropertyInfo(Variant::VECTOR3, "safe_velocity")));
}
void NavigationAgent::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_READY: {
agent_parent = Object::cast_to<Spatial>(get_parent());
NavigationServer::get_singleton()->agent_set_callback(agent, this, "_avoidance_done");
// Search the navigation node and set it
{
Navigation *nav = NULL;
Node *p = get_parent();
while (p != NULL) {
nav = Object::cast_to<Navigation>(p);
if (nav != NULL)
p = NULL;
else
p = p->get_parent();
}
set_navigation(nav);
}
set_physics_process_internal(true);
} break;
case NOTIFICATION_EXIT_TREE: {
agent_parent = NULL;
set_navigation(NULL);
set_physics_process_internal(false);
} break;
case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
if (agent_parent) {
NavigationServer::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin);
if (!target_reached) {
if (distance_to_target() < target_desired_distance) {
emit_signal("target_reached");
target_reached = true;
}
}
}
} break;
}
}
NavigationAgent::NavigationAgent() :
agent_parent(NULL),
navigation(NULL),
agent(RID()),
target_desired_distance(1.0),
navigation_height_offset(0.0),
path_max_distance(3.0),
velocity_submitted(false),
target_reached(false),
navigation_finished(true) {
agent = NavigationServer::get_singleton()->agent_create();
set_neighbor_dist(50.0);
set_max_neighbors(10);
set_time_horizon(5.0);
set_radius(1.0);
set_max_speed(10.0);
set_ignore_y(true);
}
NavigationAgent::~NavigationAgent() {
NavigationServer::get_singleton()->free(agent);
agent = RID(); // Pointless
}
void NavigationAgent::set_navigation(Navigation *p_nav) {
if (navigation == p_nav)
return; // Pointless
navigation = p_nav;
NavigationServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
}
void NavigationAgent::set_navigation_node(Node *p_nav) {
Navigation *nav = Object::cast_to<Navigation>(p_nav);
ERR_FAIL_COND(nav == NULL);
set_navigation(nav);
}
Node *NavigationAgent::get_navigation_node() const {
return Object::cast_to<Node>(navigation);
}
void NavigationAgent::set_target_desired_distance(real_t p_dd) {
target_desired_distance = p_dd;
}
void NavigationAgent::set_radius(real_t p_radius) {
radius = p_radius;
NavigationServer::get_singleton()->agent_set_radius(agent, radius);
}
void NavigationAgent::set_player_center_height(real_t p_hh) {
navigation_height_offset = p_hh;
}
void NavigationAgent::set_ignore_y(bool p_ignore_y) {
ignore_y = p_ignore_y;
NavigationServer::get_singleton()->agent_set_ignore_y(agent, ignore_y);
}
void NavigationAgent::set_neighbor_dist(real_t p_dist) {
neighbor_dist = p_dist;
NavigationServer::get_singleton()->agent_set_neighbor_dist(agent, neighbor_dist);
}
void NavigationAgent::set_max_neighbors(int p_count) {
max_neighbors = p_count;
NavigationServer::get_singleton()->agent_set_max_neighbors(agent, max_neighbors);
}
void NavigationAgent::set_time_horizon(real_t p_time) {
time_horizon = p_time;
NavigationServer::get_singleton()->agent_set_time_horizon(agent, time_horizon);
}
void NavigationAgent::set_max_speed(real_t p_max_speed) {
max_speed = p_max_speed;
NavigationServer::get_singleton()->agent_set_max_speed(agent, max_speed);
}
void NavigationAgent::set_path_max_distance(real_t p_pmd) {
path_max_distance = p_pmd;
}
real_t NavigationAgent::get_path_max_distance() {
return path_max_distance;
}
void NavigationAgent::set_target_location(Vector3 p_location) {
target_location = p_location;
navigation_path.clear();
target_reached = false;
navigation_finished = false;
}
Vector3 NavigationAgent::get_target_location() const {
return target_location;
}
Vector3 NavigationAgent::get_next_location() {
update_navigation();
if (navigation_path.size() == 0) {
ERR_FAIL_COND_V(agent_parent == NULL, Vector3());
return agent_parent->get_global_transform().origin;
} else {
return navigation_path[nav_path_index] - Vector3(0, navigation_height_offset, 0);
}
}
real_t NavigationAgent::distance_to_target() const {
ERR_FAIL_COND_V(agent_parent == NULL, 0.0);
return agent_parent->get_global_transform().origin.distance_to(target_location);
}
bool NavigationAgent::is_target_reached() const {
return target_reached;
}
bool NavigationAgent::is_target_reachable() {
return target_desired_distance >= get_final_location().distance_to(target_location);
}
bool NavigationAgent::is_navigation_finished() {
update_navigation();
return navigation_finished;
}
Vector3 NavigationAgent::get_final_location() {
update_navigation();
if (navigation_path.size() == 0) {
return Vector3();
}
return navigation_path[navigation_path.size() - 1];
}
void NavigationAgent::set_velocity(Vector3 p_velocity) {
target_velocity = p_velocity;
NavigationServer::get_singleton()->agent_set_target_velocity(agent, target_velocity);
NavigationServer::get_singleton()->agent_set_velocity(agent, prev_safe_velocity);
velocity_submitted = true;
}
void NavigationAgent::_avoidance_done(Vector3 p_new_velocity) {
prev_safe_velocity = p_new_velocity;
if (!velocity_submitted) {
target_velocity = Vector3();
return;
}
velocity_submitted = false;
emit_signal("velocity_computed", p_new_velocity);
}
String NavigationAgent::get_configuration_warning() const {
if (!Object::cast_to<Spatial>(get_parent())) {
return TTR("The NavigationAgent can be used only under a spatial node.");
}
return String();
}
void NavigationAgent::update_navigation() {
if (agent_parent == NULL) return;
if (navigation == NULL) return;
if (update_frame_id == Engine::get_singleton()->get_physics_frames()) return;
update_frame_id = Engine::get_singleton()->get_physics_frames();
Vector3 o = agent_parent->get_global_transform().origin;
bool reload_path = false;
if (NavigationServer::get_singleton()->agent_is_map_changed(agent)) {
reload_path = true;
} else if (navigation_path.size() == 0) {
reload_path = true;
} else {
// Check if too far from the navigation path
if (nav_path_index > 0) {
Vector3 segment[2];
segment[0] = navigation_path[nav_path_index - 1];
segment[1] = navigation_path[nav_path_index];
segment[0].y -= navigation_height_offset;
segment[1].y -= navigation_height_offset;
Vector3 p = Geometry::get_closest_point_to_segment(o, segment);
if (o.distance_to(p) >= path_max_distance) {
// To faraway, reload path
reload_path = true;
}
}
}
if (reload_path) {
navigation_path = NavigationServer::get_singleton()->map_get_path(navigation->get_rid(), o, target_location, true);
navigation_finished = false;
nav_path_index = 0;
emit_signal("path_changed");
}
if (navigation_path.size() == 0)
return;
// Check if we can advance the navigation path
if (navigation_finished == false) {
// Advances to the next far away location.
while (o.distance_to(navigation_path[nav_path_index] - Vector3(0, navigation_height_offset, 0)) < target_desired_distance) {
nav_path_index += 1;
if (nav_path_index == navigation_path.size()) {
nav_path_index -= 1;
navigation_finished = true;
emit_signal("navigation_finished");
break;
}
}
}
}

162
scene/3d/navigation_agent.h Normal file
View File

@ -0,0 +1,162 @@
/*************************************************************************/
/* navigation_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 NAVIGATION_AGENT_H
#define NAVIGATION_AGENT_H
#include "core/vector.h"
#include "scene/main/node.h"
class Spatial;
class Navigation;
class NavigationAgent : public Node {
GDCLASS(NavigationAgent, Node);
Spatial *agent_parent;
Navigation *navigation;
RID agent;
real_t target_desired_distance;
real_t radius;
real_t navigation_height_offset;
bool ignore_y;
real_t neighbor_dist;
int max_neighbors;
real_t time_horizon;
real_t max_speed;
real_t path_max_distance;
Vector3 target_location;
Vector<Vector3> navigation_path;
int nav_path_index;
bool velocity_submitted;
Vector3 prev_safe_velocity;
/// The submitted target velocity
Vector3 target_velocity;
bool target_reached;
bool navigation_finished;
// No initialized on purpose
uint32_t update_frame_id;
protected:
static void _bind_methods();
void _notification(int p_what);
public:
NavigationAgent();
virtual ~NavigationAgent();
void set_navigation(Navigation *p_nav);
const Navigation *get_navigation() const {
return navigation;
}
void set_navigation_node(Node *p_nav);
Node *get_navigation_node() const;
RID get_rid() const {
return agent;
}
void set_target_desired_distance(real_t p_dd);
real_t get_target_desired_distance() const {
return target_desired_distance;
}
void set_radius(real_t p_radius);
real_t get_radius() const {
return radius;
}
void set_player_center_height(real_t p_hh);
real_t get_player_center_height() const {
return navigation_height_offset;
}
void set_ignore_y(bool p_ignore_y);
bool get_ignore_y() const {
return ignore_y;
}
void set_neighbor_dist(real_t p_dist);
real_t get_neighbor_dist() const {
return neighbor_dist;
}
void set_max_neighbors(int p_count);
int get_max_neighbors() const {
return max_neighbors;
}
void set_time_horizon(real_t p_time);
real_t get_time_horizon() const {
return time_horizon;
}
void set_max_speed(real_t p_max_speed);
real_t get_max_speed() const {
return max_speed;
}
void set_path_max_distance(real_t p_pmd);
real_t get_path_max_distance();
void set_target_location(Vector3 p_location);
Vector3 get_target_location() const;
Vector3 get_next_location();
Vector<Vector3> get_nav_path() const {
return navigation_path;
}
int get_nav_path_index() const {
return nav_path_index;
}
real_t distance_to_target() const;
bool is_target_reached() const;
bool is_target_reachable();
bool is_navigation_finished();
Vector3 get_final_location();
void set_velocity(Vector3 p_velocity);
void _avoidance_done(Vector3 p_new_velocity);
virtual String get_configuration_warning() const;
private:
void update_navigation();
};
#endif

258
scene/3d/navigation_mesh_instance.cpp Normal file
View File

@ -0,0 +1,258 @@
/*************************************************************************/
/* navigation_mesh.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 "navigation_mesh_instance.h"
#include "core/os/thread.h"
#include "mesh_instance.h"
#include "navigation.h"
#include "servers/navigation_server.h"
void NavigationMeshInstance::set_enabled(bool p_enabled) {
if (enabled == p_enabled)
return;
enabled = p_enabled;
if (!is_inside_tree())
return;
if (!enabled) {
NavigationServer::get_singleton()->region_set_map(region, RID());
} else {
if (navigation) {
NavigationServer::get_singleton()->region_set_map(region, navigation->get_rid());
}
}
if (debug_view) {
MeshInstance *dm = Object::cast_to<MeshInstance>(debug_view);
if (is_enabled()) {
dm->set_material_override(get_tree()->get_debug_navigation_material());
} else {
dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
}
}
update_gizmo();
}
bool NavigationMeshInstance::is_enabled() const {
return enabled;
}
/////////////////////////////
void NavigationMeshInstance::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_ENTER_TREE: {
Spatial *c = this;
while (c) {
navigation = Object::cast_to<Navigation>(c);
if (navigation) {
if (enabled) {
NavigationServer::get_singleton()->region_set_map(region, navigation->get_rid());
}
break;
}
c = c->get_parent_spatial();
}
if (navmesh.is_valid() && get_tree()->is_debugging_navigation_hint()) {
MeshInstance *dm = memnew(MeshInstance);
dm->set_mesh(navmesh->get_debug_mesh());
if (is_enabled()) {
dm->set_material_override(get_tree()->get_debug_navigation_material());
} else {
dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
}
add_child(dm);
debug_view = dm;
}
} break;
case NOTIFICATION_TRANSFORM_CHANGED: {
NavigationServer::get_singleton()->region_set_transform(region, get_global_transform());
} break;
case NOTIFICATION_EXIT_TREE: {
if (navigation) {
NavigationServer::get_singleton()->region_set_map(region, RID());
}
if (debug_view) {
debug_view->queue_delete();
debug_view = NULL;
}
navigation = NULL;
} break;
}
}
void NavigationMeshInstance::set_navigation_mesh(const Ref<NavigationMesh> &p_navmesh) {
if (p_navmesh == navmesh)
return;
if (navmesh.is_valid()) {
navmesh->remove_change_receptor(this);
}
navmesh = p_navmesh;
if (navmesh.is_valid()) {
navmesh->add_change_receptor(this);
}
NavigationServer::get_singleton()->region_set_navmesh(region, p_navmesh);
if (debug_view && navmesh.is_valid()) {
Object::cast_to<MeshInstance>(debug_view)->set_mesh(navmesh->get_debug_mesh());
}
emit_signal("navigation_mesh_changed");
update_gizmo();
update_configuration_warning();
}
Ref<NavigationMesh> NavigationMeshInstance::get_navigation_mesh() const {
return navmesh;
}
struct BakeThreadsArgs {
NavigationMeshInstance *nav_mesh_instance;
};
void _bake_navigation_mesh(void *p_user_data) {
BakeThreadsArgs *args = static_cast<BakeThreadsArgs *>(p_user_data);
if (args->nav_mesh_instance->get_navigation_mesh().is_valid()) {
Ref<NavigationMesh> nav_mesh = args->nav_mesh_instance->get_navigation_mesh()->duplicate();
NavigationServer::get_singleton()->region_bake_navmesh(nav_mesh, args->nav_mesh_instance);
args->nav_mesh_instance->call_deferred("_bake_finished", nav_mesh);
memdelete(args);
} else {
ERR_PRINT("Can't bake the navigation mesh if the `NavigationMesh` resource doesn't exist");
args->nav_mesh_instance->call_deferred("_bake_finished", Ref<NavigationMesh>());
memdelete(args);
}
}
void NavigationMeshInstance::bake_navigation_mesh() {
ERR_FAIL_COND(bake_thread != NULL);
BakeThreadsArgs *args = memnew(BakeThreadsArgs);
args->nav_mesh_instance = this;
bake_thread = Thread::create(_bake_navigation_mesh, args);
ERR_FAIL_COND(bake_thread == NULL);
}
void NavigationMeshInstance::_bake_finished(Ref<NavigationMesh> p_nav_mesh) {
set_navigation_mesh(p_nav_mesh);
bake_thread = NULL;
}
String NavigationMeshInstance::get_configuration_warning() const {
if (!is_visible_in_tree() || !is_inside_tree())
return String();
if (!navmesh.is_valid()) {
return TTR("A NavigationMesh resource must be set or created for this node to work.");
}
const Spatial *c = this;
while (c) {
if (Object::cast_to<Navigation>(c))
return String();
c = Object::cast_to<Spatial>(c->get_parent());
}
return TTR("NavigationMeshInstance must be a child or grandchild to a Navigation node. It only provides navigation data.");
}
void NavigationMeshInstance::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_navigation_mesh", "navmesh"), &NavigationMeshInstance::set_navigation_mesh);
ClassDB::bind_method(D_METHOD("get_navigation_mesh"), &NavigationMeshInstance::get_navigation_mesh);
ClassDB::bind_method(D_METHOD("set_enabled", "enabled"), &NavigationMeshInstance::set_enabled);
ClassDB::bind_method(D_METHOD("is_enabled"), &NavigationMeshInstance::is_enabled);
ClassDB::bind_method(D_METHOD("bake_navigation_mesh"), &NavigationMeshInstance::bake_navigation_mesh);
ClassDB::bind_method(D_METHOD("_bake_finished", "nav_mesh"), &NavigationMeshInstance::_bake_finished);
ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "navmesh", PROPERTY_HINT_RESOURCE_TYPE, "NavigationMesh"), "set_navigation_mesh", "get_navigation_mesh");
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enabled"), "set_enabled", "is_enabled");
ADD_SIGNAL(MethodInfo("navigation_mesh_changed"));
ADD_SIGNAL(MethodInfo("bake_finished"));
}
void NavigationMeshInstance::_changed_callback(Object *p_changed, const char *p_prop) {
update_gizmo();
update_configuration_warning();
}
NavigationMeshInstance::NavigationMeshInstance() {
enabled = true;
set_notify_transform(true);
region = NavigationServer::get_singleton()->region_create();
navigation = NULL;
debug_view = NULL;
bake_thread = NULL;
}
NavigationMeshInstance::~NavigationMeshInstance() {
if (navmesh.is_valid())
navmesh->remove_change_receptor(this);
NavigationServer::get_singleton()->free(region);
}

75
scene/3d/navigation_mesh_instance.h Normal file
View File

@ -0,0 +1,75 @@
/*************************************************************************/
/* navigation_mesh.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_INSTANCE_H
#define NAVIGATION_MESH_INSTANCE_H
#include "scene/3d/spatial.h"
#include "scene/resources/mesh.h"
#include "scene/resources/navigation_mesh.h"
class Navigation;
class NavigationMeshInstance : public Spatial {
GDCLASS(NavigationMeshInstance, Spatial);
bool enabled;
RID region;
Ref<NavigationMesh> navmesh;
Navigation *navigation;
Node *debug_view;
Thread *bake_thread;
protected:
void _notification(int p_what);
static void _bind_methods();
void _changed_callback(Object *p_changed, const char *p_prop);
public:
void set_enabled(bool p_enabled);
bool is_enabled() const;
void set_navigation_mesh(const Ref<NavigationMesh> &p_navmesh);
Ref<NavigationMesh> get_navigation_mesh() const;
/// Bakes the navigation mesh in a dedicated thread; once done, automatically
/// sets the new navigation mesh and emits a signal
void bake_navigation_mesh();
void _bake_finished(Ref<NavigationMesh> p_nav_mesh);
String get_configuration_warning() const;
NavigationMeshInstance();
~NavigationMeshInstance();
};
#endif // NAVIGATION_MESH_INSTANCE_H

163
scene/3d/navigation_obstacle.cpp Normal file
View File

@ -0,0 +1,163 @@
/*************************************************************************/
/* navigation_obstacle.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 "navigation_obstacle.h"
#include "scene/3d/collision_shape.h"
#include "scene/3d/navigation.h"
#include "scene/3d/physics_body.h"
#include "servers/navigation_server.h"
void NavigationObstacle::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_navigation", "navigation"), &NavigationObstacle::set_navigation_node);
ClassDB::bind_method(D_METHOD("get_navigation"), &NavigationObstacle::get_navigation_node);
}
void NavigationObstacle::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_READY: {
update_agent_shape();
// Search the navigation node and set it
{
Navigation *nav = NULL;
Node *p = get_parent();
while (p != NULL) {
nav = Object::cast_to<Navigation>(p);
if (nav != NULL)
p = NULL;
else
p = p->get_parent();
}
set_navigation(nav);
}
set_physics_process_internal(true);
} break;
case NOTIFICATION_EXIT_TREE: {
set_navigation(NULL);
set_physics_process_internal(false);
} break;
case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
Spatial *spatial = Object::cast_to<Spatial>(get_parent());
if (spatial) {
NavigationServer::get_singleton()->agent_set_position(agent, spatial->get_global_transform().origin);
}
PhysicsBody *rigid = Object::cast_to<PhysicsBody>(get_parent());
if (rigid) {
Vector3 v = rigid->get_linear_velocity();
NavigationServer::get_singleton()->agent_set_velocity(agent, v);
NavigationServer::get_singleton()->agent_set_target_velocity(agent, v);
}
} break;
}
}
NavigationObstacle::NavigationObstacle() :
navigation(NULL),
agent(RID()) {
agent = NavigationServer::get_singleton()->agent_create();
}
NavigationObstacle::~NavigationObstacle() {
NavigationServer::get_singleton()->free(agent);
agent = RID(); // Pointless
}
void NavigationObstacle::set_navigation(Navigation *p_nav) {
if (navigation == p_nav)
return; // Pointless
navigation = p_nav;
NavigationServer::get_singleton()->agent_set_map(agent, navigation == NULL ? RID() : navigation->get_rid());
}
void NavigationObstacle::set_navigation_node(Node *p_nav) {
Navigation *nav = Object::cast_to<Navigation>(p_nav);
ERR_FAIL_COND(nav == NULL);
set_navigation(nav);
}
Node *NavigationObstacle::get_navigation_node() const {
return Object::cast_to<Node>(navigation);
}
String NavigationObstacle::get_configuration_warning() const {
if (!Object::cast_to<Spatial>(get_parent())) {
return TTR("The NavigationObstacle only serves to provide collision avoidance to a spatial object.");
}
return String();
}
void NavigationObstacle::update_agent_shape() {
Node *node = get_parent();
// Estimate the radius of this physics body
real_t radius = 0.0;
for (int i(0); i < node->get_child_count(); i++) {
// For each collision shape
CollisionShape *cs = Object::cast_to<CollisionShape>(node->get_child(i));
if (cs) {
// Take the distance between the Body center to the shape center
real_t r = cs->get_transform().origin.length();
if (cs->get_shape().is_valid()) {
// and add the enclosing shape radius
r += cs->get_shape()->get_enclosing_radius();
}
Vector3 s = cs->get_global_transform().basis.get_scale();
r *= MAX(s.x, MAX(s.y, s.z));
// Takes the biggest radius
radius = MAX(radius, r);
}
}
Spatial *spa = Object::cast_to<Spatial>(node);
if (spa) {
Vector3 s = spa->get_global_transform().basis.get_scale();
radius *= MAX(s.x, MAX(s.y, s.z));
}
if (radius == 0.0)
radius = 1.0; // Never a 0 radius
// Initialize the Agent as an object
NavigationServer::get_singleton()->agent_set_neighbor_dist(agent, 0.0);
NavigationServer::get_singleton()->agent_set_max_neighbors(agent, 0);
NavigationServer::get_singleton()->agent_set_time_horizon(agent, 0.0);
NavigationServer::get_singleton()->agent_set_radius(agent, radius);
NavigationServer::get_singleton()->agent_set_max_speed(agent, 0.0);
}

71
scene/3d/navigation_obstacle.h Normal file
View File

@ -0,0 +1,71 @@
/*************************************************************************/
/* navigation_obstacle.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_OBSTACLE_H
#define NAVIGATION_OBSTACLE_H
#include "scene/main/node.h"
class Navigation;
class NavigationObstacle : public Node {
GDCLASS(NavigationObstacle, Node);
Navigation *navigation;
RID agent;
protected:
static void _bind_methods();
void _notification(int p_what);
public:
NavigationObstacle();
virtual ~NavigationObstacle();
void set_navigation(Navigation *p_nav);
const Navigation *get_navigation() const {
return navigation;
}
void set_navigation_node(Node *p_nav);
Node *get_navigation_node() const;
RID get_rid() const {
return agent;
}
virtual String get_configuration_warning() const;
private:
void update_agent_shape();
};
#endif

28
scene/3d/physics_body.cpp
View File

@ -36,15 +36,14 @@
#include "core/method_bind_ext.gen.inc"
#include "core/object.h"
#include "core/rid.h"
#include "scene/3d/collision_shape.h"
#include "scene/scene_string_names.h"
#include "servers/navigation_server.h"
#ifdef TOOLS_ENABLED
#include "editor/plugins/spatial_editor_plugin.h"
#endif
void PhysicsBody::_notification(int p_what) {
}
Vector3 PhysicsBody::get_linear_velocity() const {
return Vector3();
@ -1104,6 +1103,14 @@ Ref<KinematicCollision> KinematicBody::_move(const Vector3 &p_motion, bool p_inf
return Ref<KinematicCollision>();
}
Vector3 KinematicBody::get_linear_velocity() const {
return linear_velocity;
}
Vector3 KinematicBody::get_angular_velocity() const {
return angular_velocity;
}
bool KinematicBody::move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes, bool p_test_only) {
Transform gt = get_global_transform();
@ -1399,6 +1406,8 @@ void KinematicBody::_notification(int p_what) {
void KinematicBody::_bind_methods() {
ClassDB::bind_method(D_METHOD("_direct_state_changed"), &KinematicBody::_direct_state_changed);
ClassDB::bind_method(D_METHOD("move_and_collide", "rel_vec", "infinite_inertia", "exclude_raycast_shapes", "test_only"), &KinematicBody::_move, DEFVAL(true), DEFVAL(true), DEFVAL(false));
ClassDB::bind_method(D_METHOD("move_and_slide", "linear_velocity", "up_direction", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true));
ClassDB::bind_method(D_METHOD("move_and_slide_with_snap", "linear_velocity", "snap", "up_direction", "stop_on_slope", "max_slides", "floor_max_angle", "infinite_inertia"), &KinematicBody::move_and_slide_with_snap, DEFVAL(Vector3(0, 0, 0)), DEFVAL(false), DEFVAL(4), DEFVAL(Math::deg2rad((float)45)), DEFVAL(true));
@ -1427,6 +1436,17 @@ void KinematicBody::_bind_methods() {
ADD_PROPERTY(PropertyInfo(Variant::REAL, "collision/safe_margin", PROPERTY_HINT_RANGE, "0.001,256,0.001"), "set_safe_margin", "get_safe_margin");
}
void KinematicBody::_direct_state_changed(Object *p_state) {
#ifdef DEBUG_ENABLED
PhysicsDirectBodyState *state = Object::cast_to<PhysicsDirectBodyState>(p_state);
#else
PhysicsDirectBodyState *state = (PhysicsDirectBodyState *)p_state; //trust it
#endif
linear_velocity = state->get_linear_velocity();
angular_velocity = state->get_angular_velocity();
}
KinematicBody::KinematicBody() :
PhysicsBody(PhysicsServer::BODY_MODE_KINEMATIC) {
@ -1435,6 +1455,8 @@ KinematicBody::KinematicBody() :
on_floor = false;
on_ceiling = false;
on_wall = false;
PhysicsServer::get_singleton()->body_set_force_integration_callback(get_rid(), this, "_direct_state_changed");
}
KinematicBody::~KinematicBody() {

9
scene/3d/physics_body.h
View File

@ -49,7 +49,6 @@ class PhysicsBody : public CollisionObject {
protected:
static void _bind_methods();
void _notification(int p_what);
PhysicsBody(PhysicsServer::BodyMode p_mode);
public:
@ -296,6 +295,9 @@ public:
};
private:
Vector3 linear_velocity;
Vector3 angular_velocity;
uint16_t locked_axis;
float margin;
@ -319,7 +321,12 @@ protected:
void _notification(int p_what);
static void _bind_methods();
virtual void _direct_state_changed(Object *p_state);
public:
virtual Vector3 get_linear_velocity() const;
virtual Vector3 get_angular_velocity() const;
bool move_and_collide(const Vector3 &p_motion, bool p_infinite_inertia, Collision &r_collision, bool p_exclude_raycast_shapes = true, bool p_test_only = false);
bool test_move(const Transform &p_from, const Vector3 &p_motion, bool p_infinite_inertia);

2
scene/main/scene_tree.cpp
View File

@ -46,6 +46,7 @@
#include "scene/resources/mesh.h"
#include "scene/resources/packed_scene.h"
#include "scene/scene_string_names.h"
#include "servers/navigation_server.h"
#include "servers/physics_2d_server.h"
#include "servers/physics_server.h"
#include "viewport.h"
@ -896,6 +897,7 @@ void SceneTree::set_pause(bool p_enabled) {
if (p_enabled == pause)
return;
pause = p_enabled;
NavigationServer::get_singleton()->set_active(!p_enabled);
PhysicsServer::get_singleton()->set_active(!p_enabled);
Physics2DServer::get_singleton()->set_active(!p_enabled);
if (get_root())

23
scene/register_scene_types.cpp
View File

@ -50,6 +50,8 @@
#include "scene/2d/mesh_instance_2d.h"
#include "scene/2d/multimesh_instance_2d.h"
#include "scene/2d/navigation_2d.h"
#include "scene/2d/navigation_agent_2d.h"
#include "scene/2d/navigation_obstacle_2d.h"
#include "scene/2d/parallax_background.h"
#include "scene/2d/parallax_layer.h"
#include "scene/2d/particles_2d.h"
@ -145,6 +147,7 @@
#include "scene/resources/material.h"
#include "scene/resources/mesh.h"
#include "scene/resources/mesh_data_tool.h"
#include "scene/resources/navigation_mesh.h"
#include "scene/resources/packed_scene.h"
#include "scene/resources/particles_material.h"
#include "scene/resources/physics_material.h"
@ -168,9 +171,6 @@
#include "scene/resources/world_2d.h"
#include "scene/scene_string_names.h"
#include "scene/3d/spatial.h"
#include "scene/3d/world_environment.h"
#ifndef _3D_DISABLED
#include "scene/3d/area.h"
#include "scene/3d/arvr_nodes.h"
@ -189,7 +189,9 @@
#include "scene/3d/mesh_instance.h"
#include "scene/3d/multimesh_instance.h"
#include "scene/3d/navigation.h"
#include "scene/3d/navigation_mesh.h"
#include "scene/3d/navigation_agent.h"
#include "scene/3d/navigation_mesh_instance.h"
#include "scene/3d/navigation_obstacle.h"
#include "scene/3d/particles.h"
#include "scene/3d/path.h"
#include "scene/3d/physics_body.h"
@ -201,10 +203,12 @@
#include "scene/3d/remote_transform.h"
#include "scene/3d/skeleton.h"
#include "scene/3d/soft_body.h"
#include "scene/3d/spatial.h"
#include "scene/3d/spring_arm.h"
#include "scene/3d/sprite_3d.h"
#include "scene/3d/vehicle_body.h"
#include "scene/3d/visibility_notifier.h"
#include "scene/3d/world_environment.h"
#include "scene/animation/skeleton_ik.h"
#include "scene/resources/environment.h"
#include "scene/resources/mesh_library.h"
@ -422,9 +426,6 @@ void register_scene_types() {
ClassDB::register_class<Particles>();
ClassDB::register_class<CPUParticles>();
ClassDB::register_class<Position3D>();
ClassDB::register_class<NavigationMeshInstance>();
ClassDB::register_class<NavigationMesh>();
ClassDB::register_class<Navigation>();
ClassDB::register_class<RootMotionView>();
ClassDB::set_class_enabled("RootMotionView", false); //disabled by default, enabled by editor
@ -469,9 +470,15 @@ void register_scene_types() {
ClassDB::register_class<ConeTwistJoint>();
ClassDB::register_class<Generic6DOFJoint>();
ClassDB::register_class<Navigation>();
ClassDB::register_class<NavigationMeshInstance>();
ClassDB::register_class<NavigationAgent>();
ClassDB::register_class<NavigationObstacle>();
OS::get_singleton()->yield(); //may take time to init
#endif
ClassDB::register_class<NavigationMesh>();
AcceptDialog::set_swap_ok_cancel(GLOBAL_DEF("gui/common/swap_ok_cancel", bool(OS::get_singleton()->get_swap_ok_cancel())));
@ -713,6 +720,8 @@ void register_scene_types() {
ClassDB::register_class<Navigation2D>();
ClassDB::register_class<NavigationPolygon>();
ClassDB::register_class<NavigationPolygonInstance>();
ClassDB::register_class<NavigationAgent2D>();
ClassDB::register_class<NavigationObstacle2D>();
OS::get_singleton()->yield(); //may take time to init

4
scene/resources/box_shape.cpp
View File

@ -48,6 +48,10 @@ Vector<Vector3> BoxShape::get_debug_mesh_lines() {
return lines;
}
real_t BoxShape::get_enclosing_radius() const {
return extents.length();
}
void BoxShape::_update_shape() {
PhysicsServer::get_singleton()->shape_set_data(get_shape(), extents);

1
scene/resources/box_shape.h
View File

@ -48,6 +48,7 @@ public:
Vector3 get_extents() const;
virtual Vector<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
BoxShape();
};

4
scene/resources/capsule_shape.cpp
View File

@ -69,6 +69,10 @@ Vector<Vector3> CapsuleShape::get_debug_mesh_lines() {
return points;
}
real_t CapsuleShape::get_enclosing_radius() const {
return radius + height * 0.5;
}
void CapsuleShape::_update_shape() {
Dictionary d;

1
scene/resources/capsule_shape.h
View File

@ -51,6 +51,7 @@ public:
float get_height() const;
virtual Vector<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
CapsuleShape();
};

4
scene/resources/capsule_shape_2d.cpp
View File

@ -97,6 +97,10 @@ Rect2 CapsuleShape2D::get_rect() const {
return rect;
}
real_t CapsuleShape2D::get_enclosing_radius() const {
return radius + height * 0.5;
}
void CapsuleShape2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_radius", "radius"), &CapsuleShape2D::set_radius);

1
scene/resources/capsule_shape_2d.h
View File

@ -56,6 +56,7 @@ public:
virtual void draw(const RID &p_to_rid, const Color &p_color);
virtual Rect2 get_rect() const;
virtual real_t get_enclosing_radius() const;
CapsuleShape2D();
};

4
scene/resources/circle_shape_2d.cpp
View File

@ -70,6 +70,10 @@ Rect2 CircleShape2D::get_rect() const {
return rect;
}
real_t CircleShape2D::get_enclosing_radius() const {
return radius;
}
void CircleShape2D::draw(const RID &p_to_rid, const Color &p_color) {
Vector<Vector2> points;

1
scene/resources/circle_shape_2d.h
View File

@ -50,6 +50,7 @@ public:
virtual void draw(const RID &p_to_rid, const Color &p_color);
virtual Rect2 get_rect() const;
virtual real_t get_enclosing_radius() const;
CircleShape2D();
};

10
scene/resources/concave_polygon_shape.cpp
View File

@ -64,6 +64,16 @@ Vector<Vector3> ConcavePolygonShape::get_debug_mesh_lines() {
return points;
}
real_t ConcavePolygonShape::get_enclosing_radius() const {
PoolVector<Vector3> data = get_faces();
PoolVector<Vector3>::Read read = data.read();
real_t r = 0;
for (int i(0); i < data.size(); i++) {
r = MAX(read[i].length_squared(), r);
}
return Math::sqrt(r);
}
void ConcavePolygonShape::_update_shape() {
Shape::_update_shape();
}

3
scene/resources/concave_polygon_shape.h
View File

@ -66,7 +66,8 @@ public:
void set_faces(const PoolVector<Vector3> &p_faces);
PoolVector<Vector3> get_faces() const;
Vector<Vector3> get_debug_mesh_lines();
virtual Vector<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
ConcavePolygonShape();
};

10
scene/resources/concave_polygon_shape_2d.cpp
View File

@ -94,6 +94,16 @@ Rect2 ConcavePolygonShape2D::get_rect() const {
return rect;
}
real_t ConcavePolygonShape2D::get_enclosing_radius() const {
PoolVector<Vector2> data = get_segments();
PoolVector<Vector2>::Read read = data.read();
real_t r = 0;
for (int i(0); i < data.size(); i++) {
r = MAX(read[i].length_squared(), r);
}
return Math::sqrt(r);
}
void ConcavePolygonShape2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_segments", "segments"), &ConcavePolygonShape2D::set_segments);

1
scene/resources/concave_polygon_shape_2d.h
View File

@ -47,6 +47,7 @@ public:
virtual void draw(const RID &p_to_rid, const Color &p_color);
virtual Rect2 get_rect() const;
virtual real_t get_enclosing_radius() const;
ConcavePolygonShape2D();
};

10
scene/resources/convex_polygon_shape.cpp
View File

@ -55,6 +55,16 @@ Vector<Vector3> ConvexPolygonShape::get_debug_mesh_lines() {
return Vector<Vector3>();
}
real_t ConvexPolygonShape::get_enclosing_radius() const {
PoolVector<Vector3> data = get_points();
PoolVector<Vector3>::Read read = data.read();
real_t r = 0;
for (int i(0); i < data.size(); i++) {
r = MAX(read[i].length_squared(), r);
}
return Math::sqrt(r);
}
void ConvexPolygonShape::_update_shape() {
PhysicsServer::get_singleton()->shape_set_data(get_shape(), points);

1
scene/resources/convex_polygon_shape.h
View File

@ -48,6 +48,7 @@ public:
PoolVector<Vector3> get_points() const;
virtual Vector<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
ConvexPolygonShape();
};

8
scene/resources/convex_polygon_shape_2d.cpp
View File

@ -97,6 +97,14 @@ Rect2 ConvexPolygonShape2D::get_rect() const {
return rect;
}
real_t ConvexPolygonShape2D::get_enclosing_radius() const {
real_t r = 0;
for (int i(0); i < get_points().size(); i++) {
r = MAX(get_points()[i].length_squared(), r);
}
return Math::sqrt(r);
}
ConvexPolygonShape2D::ConvexPolygonShape2D() :
Shape2D(Physics2DServer::get_singleton()->convex_polygon_shape_create()) {
}

1
scene/resources/convex_polygon_shape_2d.h
View File

@ -51,6 +51,7 @@ public:
virtual void draw(const RID &p_to_rid, const Color &p_color);
virtual Rect2 get_rect() const;
virtual real_t get_enclosing_radius() const;
ConvexPolygonShape2D();
};

4
scene/resources/cylinder_shape.cpp
View File

@ -62,6 +62,10 @@ Vector<Vector3> CylinderShape::get_debug_mesh_lines() {
return points;
}
real_t CylinderShape::get_enclosing_radius() const {
return Vector2(radius, height * 0.5).length();
}
void CylinderShape::_update_shape() {
Dictionary d;

1
scene/resources/cylinder_shape.h
View File

@ -50,6 +50,7 @@ public:
float get_height() const;
virtual Vector<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
CylinderShape();
};

4
scene/resources/height_map_shape.cpp
View File

@ -76,6 +76,10 @@ Vector<Vector3> HeightMapShape::get_debug_mesh_lines() {
return points;
}
real_t HeightMapShape::get_enclosing_radius() const {
return Vector3(real_t(map_width), max_height - min_height, real_t(map_depth)).length();
}
void HeightMapShape::_update_shape() {
Dictionary d;

1
scene/resources/height_map_shape.h
View File

@ -55,6 +55,7 @@ public:
PoolRealArray get_map_data() const;
virtual Vector<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
HeightMapShape();
};

4
scene/resources/line_shape_2d.cpp
View File

@ -100,6 +100,10 @@ Rect2 LineShape2D::get_rect() const {
return rect;
}
real_t LineShape2D::get_enclosing_radius() const {
return d;
}
void LineShape2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_normal", "normal"), &LineShape2D::set_normal);

1
scene/resources/line_shape_2d.h
View File

@ -55,6 +55,7 @@ public:
virtual void draw(const RID &p_to_rid, const Color &p_color);
virtual Rect2 get_rect() const;
virtual real_t get_enclosing_radius() const;
LineShape2D();
};

2
scene/resources/mesh_library.h
View File

@ -34,7 +34,7 @@
#include "core/map.h"
#include "core/resource.h"
#include "mesh.h"
#include "scene/3d/navigation_mesh.h"
#include "scene/3d/navigation_mesh_instance.h"
#include "shape.h"
class MeshLibrary : public Resource {

196
scene/3d/navigation_mesh.cpp → scene/resources/navigation_mesh.cpp
View File

@ -29,8 +29,6 @@
/*************************************************************************/
#include "navigation_mesh.h"
#include "mesh_instance.h"
#include "navigation.h"
void NavigationMesh::create_from_mesh(const Ref<Mesh> &p_mesh) {
@ -548,197 +546,3 @@ NavigationMesh::NavigationMesh() {
filter_ledge_spans = false;
filter_walkable_low_height_spans = false;
}
void NavigationMeshInstance::set_enabled(bool p_enabled) {
if (enabled == p_enabled)
return;
enabled = p_enabled;
if (!is_inside_tree())
return;
if (!enabled) {
if (nav_id != -1) {
navigation->navmesh_remove(nav_id);
nav_id = -1;
}
} else {
if (navigation) {
if (navmesh.is_valid()) {
nav_id = navigation->navmesh_add(navmesh, get_relative_transform(navigation), this);
}
}
}
if (debug_view) {
MeshInstance *dm = Object::cast_to<MeshInstance>(debug_view);
if (is_enabled()) {
dm->set_material_override(get_tree()->get_debug_navigation_material());
} else {
dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
}
}
update_gizmo();
}
bool NavigationMeshInstance::is_enabled() const {
return enabled;
}
/////////////////////////////
void NavigationMeshInstance::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_ENTER_TREE: {
Spatial *c = this;
while (c) {
navigation = Object::cast_to<Navigation>(c);
if (navigation) {
if (enabled && navmesh.is_valid()) {
nav_id = navigation->navmesh_add(navmesh, get_relative_transform(navigation), this);
}
break;
}
c = c->get_parent_spatial();
}
if (navmesh.is_valid() && get_tree()->is_debugging_navigation_hint()) {
MeshInstance *dm = memnew(MeshInstance);
dm->set_mesh(navmesh->get_debug_mesh());
if (is_enabled()) {
dm->set_material_override(get_tree()->get_debug_navigation_material());
} else {
dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
}
add_child(dm);
debug_view = dm;
}
} break;
case NOTIFICATION_TRANSFORM_CHANGED: {
if (navigation && nav_id != -1) {
navigation->navmesh_set_transform(nav_id, get_relative_transform(navigation));
}
} break;
case NOTIFICATION_EXIT_TREE: {
if (navigation) {
if (nav_id != -1) {
navigation->navmesh_remove(nav_id);
nav_id = -1;
}
}
if (debug_view) {
debug_view->queue_delete();
debug_view = NULL;
}
navigation = NULL;
} break;
}
}
void NavigationMeshInstance::set_navigation_mesh(const Ref<NavigationMesh> &p_navmesh) {
if (p_navmesh == navmesh)
return;
if (navigation && nav_id != -1) {
navigation->navmesh_remove(nav_id);
nav_id = -1;
}
if (navmesh.is_valid()) {
navmesh->remove_change_receptor(this);
}
navmesh = p_navmesh;
if (navmesh.is_valid()) {
navmesh->add_change_receptor(this);
}
if (navigation && navmesh.is_valid() && enabled) {
nav_id = navigation->navmesh_add(navmesh, get_relative_transform(navigation), this);
}
if (debug_view && navmesh.is_valid()) {
Object::cast_to<MeshInstance>(debug_view)->set_mesh(navmesh->get_debug_mesh());
}
update_gizmo();
update_configuration_warning();
}
Ref<NavigationMesh> NavigationMeshInstance::get_navigation_mesh() const {
return navmesh;
}
String NavigationMeshInstance::get_configuration_warning() const {
if (!is_visible_in_tree() || !is_inside_tree())
return String();
if (!navmesh.is_valid()) {
return TTR("A NavigationMesh resource must be set or created for this node to work.");
}
const Spatial *c = this;
while (c) {
if (Object::cast_to<Navigation>(c))
return String();
c = Object::cast_to<Spatial>(c->get_parent());
}
return TTR("NavigationMeshInstance must be a child or grandchild to a Navigation node. It only provides navigation data.");
}
void NavigationMeshInstance::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_navigation_mesh", "navmesh"), &NavigationMeshInstance::set_navigation_mesh);
ClassDB::bind_method(D_METHOD("get_navigation_mesh"), &NavigationMeshInstance::get_navigation_mesh);
ClassDB::bind_method(D_METHOD("set_enabled", "enabled"), &NavigationMeshInstance::set_enabled);
ClassDB::bind_method(D_METHOD("is_enabled"), &NavigationMeshInstance::is_enabled);
ADD_PROPERTY(PropertyInfo(Variant::OBJECT, "navmesh", PROPERTY_HINT_RESOURCE_TYPE, "NavigationMesh"), "set_navigation_mesh", "get_navigation_mesh");
ADD_PROPERTY(PropertyInfo(Variant::BOOL, "enabled"), "set_enabled", "is_enabled");
}
void NavigationMeshInstance::_changed_callback(Object *p_changed, const char *p_prop) {
update_gizmo();
update_configuration_warning();
}
NavigationMeshInstance::NavigationMeshInstance() {
debug_view = NULL;
navigation = NULL;
nav_id = -1;
enabled = true;
set_notify_transform(true);
}
NavigationMeshInstance::~NavigationMeshInstance() {
if (navmesh.is_valid())
navmesh->remove_change_receptor(this);
}

32
scene/3d/navigation_mesh.h → scene/resources/navigation_mesh.h
View File

@ -31,7 +31,6 @@
#ifndef NAVIGATION_MESH_H
#define NAVIGATION_MESH_H
#include "scene/3d/spatial.h"
#include "scene/resources/mesh.h"
class Mesh;
@ -193,35 +192,4 @@ public:
NavigationMesh();
};
class Navigation;
class NavigationMeshInstance : public Spatial {
GDCLASS(NavigationMeshInstance, Spatial);
bool enabled;
int nav_id;
Navigation *navigation;
Ref<NavigationMesh> navmesh;
Node *debug_view;
protected:
void _notification(int p_what);
static void _bind_methods();
void _changed_callback(Object *p_changed, const char *p_prop);
public:
void set_enabled(bool p_enabled);
bool is_enabled() const;
void set_navigation_mesh(const Ref<NavigationMesh> &p_navmesh);
Ref<NavigationMesh> get_navigation_mesh() const;
String get_configuration_warning() const;
NavigationMeshInstance();
~NavigationMeshInstance();
};
#endif // NAVIGATION_MESH_H

4
scene/resources/plane_shape.h
View File

@ -47,6 +47,10 @@ public:
Plane get_plane() const;
virtual Vector<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const {
// Should be infinite?
return 0;
}
PlaneShape();
};

4
scene/resources/ray_shape.cpp
View File

@ -41,6 +41,10 @@ Vector<Vector3> RayShape::get_debug_mesh_lines() {
return points;
}
real_t RayShape::get_enclosing_radius() const {
return length;
}
void RayShape::_update_shape() {
Dictionary d;

1
scene/resources/ray_shape.h
View File

@ -50,6 +50,7 @@ public:
bool get_slips_on_slope() const;
virtual Vector<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
RayShape();
};

4
scene/resources/rectangle_shape_2d.cpp
View File

@ -59,6 +59,10 @@ Rect2 RectangleShape2D::get_rect() const {
return Rect2(-extents, extents * 2.0);
}
real_t RectangleShape2D::get_enclosing_radius() const {
return extents.length();
}
void RectangleShape2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_extents", "extents"), &RectangleShape2D::set_extents);

1
scene/resources/rectangle_shape_2d.h
View File

@ -48,6 +48,7 @@ public:
virtual void draw(const RID &p_to_rid, const Color &p_color);
virtual Rect2 get_rect() const;
virtual real_t get_enclosing_radius() const;
RectangleShape2D();
};

8
scene/resources/segment_shape_2d.cpp
View File

@ -82,6 +82,10 @@ Rect2 SegmentShape2D::get_rect() const {
return rect;
}
real_t SegmentShape2D::get_enclosing_radius() const {
return (a + b).length();
}
void SegmentShape2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_a", "a"), &SegmentShape2D::set_a);
@ -138,6 +142,10 @@ Rect2 RayShape2D::get_rect() const {
return rect;
}
real_t RayShape2D::get_enclosing_radius() const {
return length;
}
void RayShape2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_length", "length"), &RayShape2D::set_length);

2
scene/resources/segment_shape_2d.h
View File

@ -55,6 +55,7 @@ public:
virtual void draw(const RID &p_to_rid, const Color &p_color);
virtual Rect2 get_rect() const;
virtual real_t get_enclosing_radius() const;
SegmentShape2D();
};
@ -79,6 +80,7 @@ public:
virtual void draw(const RID &p_to_rid, const Color &p_color);
virtual Rect2 get_rect() const;
virtual real_t get_enclosing_radius() const;
RayShape2D();
};

3
scene/resources/shape.h
View File

@ -32,6 +32,7 @@
#define SHAPE_H
#include "core/resource.h"
class ArrayMesh;
class Shape : public Resource {
@ -57,6 +58,8 @@ public:
Ref<ArrayMesh> get_debug_mesh();
virtual Vector<Vector3> get_debug_mesh_lines() = 0; // { return Vector<Vector3>(); }
/// Returns the radius of a sphere that fully enclose this shape
virtual real_t get_enclosing_radius() const = 0;
void add_vertices_to_array(PoolVector<Vector3> &array, const Transform &p_xform);

2
scene/resources/shape_2d.h
View File

@ -58,6 +58,8 @@ public:
virtual void draw(const RID &p_to_rid, const Color &p_color) {}
virtual Rect2 get_rect() const { return Rect2(); }
/// Returns the radius of a circle that fully enclose this shape
virtual real_t get_enclosing_radius() const = 0;
virtual RID get_rid() const;
Shape2D();
~Shape2D();

4
scene/resources/sphere_shape.cpp
View File

@ -55,6 +55,10 @@ Vector<Vector3> SphereShape::get_debug_mesh_lines() {
return points;
}
real_t SphereShape::get_enclosing_radius() const {
return radius;
}
void SphereShape::_update_shape() {
PhysicsServer::get_singleton()->shape_set_data(get_shape(), radius);

1
scene/resources/sphere_shape.h
View File

@ -48,6 +48,7 @@ public:
float get_radius() const;
virtual Vector<Vector3> get_debug_mesh_lines();
virtual real_t get_enclosing_radius() const;
SphereShape();
};

1
scene/resources/world.cpp
View File

@ -262,6 +262,7 @@ RID World::get_space() const {
return space;
}
RID World::get_scenario() const {
return scenario;

218
servers/navigation_2d_server.cpp Normal file
View File

@ -0,0 +1,218 @@
/*************************************************************************/
/* navigation_2d_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 "servers/navigation_2d_server.h"
#include "core/math/transform.h"
#include "core/math/transform_2d.h"
#include "servers/navigation_server.h"
/**
@author AndreaCatania
*/
Navigation2DServer *Navigation2DServer::singleton = NULL;
#define FORWARD_0_C(FUNC_NAME) \
Navigation2DServer::FUNC_NAME() \
const { \
return NavigationServer::get_singleton()->FUNC_NAME(); \
}
#define FORWARD_1(FUNC_NAME, T_0, D_0, CONV_0) \
Navigation2DServer::FUNC_NAME(T_0 D_0) { \
return NavigationServer::get_singleton_mut()->FUNC_NAME(CONV_0(D_0)); \
}
#define FORWARD_1_C(FUNC_NAME, T_0, D_0, CONV_0) \
Navigation2DServer::FUNC_NAME(T_0 D_0) \
const { \
return NavigationServer::get_singleton()->FUNC_NAME(CONV_0(D_0)); \
}
#define FORWARD_2_C(FUNC_NAME, T_0, D_0, T_1, D_1, CONV_0, CONV_1) \
Navigation2DServer::FUNC_NAME(T_0 D_0, T_1 D_1) \
const { \
return NavigationServer::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1)); \
}
#define FORWARD_4_R_C(CONV_R, FUNC_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3, CONV_0, CONV_1, CONV_2, CONV_3) \
Navigation2DServer::FUNC_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3) \
const { \
return CONV_R(NavigationServer::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1), CONV_2(D_2), CONV_3(D_3))); \
}
#define FORWARD_4_C(FUNC_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3, CONV_0, CONV_1, CONV_2, CONV_3) \
Navigation2DServer::FUNC_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3) \
const { \
return NavigationServer::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1), CONV_2(D_2), CONV_3(D_3)); \
}
RID rid_to_rid(const RID d) {
return d;
}
bool bool_to_bool(const bool d) {
return d;
}
int int_to_int(const int d) {
return d;
}
real_t real_to_real(const real_t d) {
return d;
}
Vector3 v2_to_v3(const Vector2 d) {
return Vector3(d.x, 0.0, d.y);
}
Vector2 v3_to_v2(const Vector3 &d) {
return Vector2(d.x, d.z);
}
Vector<Vector2> vector_v3_to_v2(const Vector<Vector3> &d) {
Vector<Vector2> nd;
nd.resize(d.size());
for (int i(0); i < nd.size(); i++) {
nd.write[i] = v3_to_v2(d[i]);
}
return nd;
}
Transform trf2_to_trf3(const Transform2D &d) {
Vector3 o(v2_to_v3(d.get_origin()));
Basis b;
b.rotate(Vector3(0, 1, 0), d.get_rotation());
return Transform(b, o);
}
Object *obj_to_obj(Object *d) {
return d;
}
StringName sn_to_sn(StringName &d) {
return d;
}
Variant var_to_var(Variant &d) {
return d;
}
Ref<NavigationMesh> poly_to_mesh(Ref<NavigationPolygon> d) {
if (d.is_valid()) {
return d->get_mesh();
} else {
return Ref<NavigationMesh>();
}
}
void Navigation2DServer::_bind_methods() {
ClassDB::bind_method(D_METHOD("map_create"), &Navigation2DServer::map_create);
ClassDB::bind_method(D_METHOD("map_set_active", "map", "active"), &Navigation2DServer::map_set_active);
ClassDB::bind_method(D_METHOD("map_is_active", "nap"), &Navigation2DServer::map_is_active);
ClassDB::bind_method(D_METHOD("map_set_cell_size", "map", "cell_size"), &Navigation2DServer::map_set_cell_size);
ClassDB::bind_method(D_METHOD("map_get_cell_size", "map"), &Navigation2DServer::map_get_cell_size);
ClassDB::bind_method(D_METHOD("map_set_edge_connection_margin", "map", "margin"), &Navigation2DServer::map_set_edge_connection_margin);
ClassDB::bind_method(D_METHOD("map_get_edge_connection_margin", "map"), &Navigation2DServer::map_get_edge_connection_margin);
ClassDB::bind_method(D_METHOD("map_get_path", "map", "origin", "destination", "optimize"), &Navigation2DServer::map_get_path);
ClassDB::bind_method(D_METHOD("region_create"), &Navigation2DServer::region_create);
ClassDB::bind_method(D_METHOD("region_set_map", "region", "map"), &Navigation2DServer::region_set_map);
ClassDB::bind_method(D_METHOD("region_set_transform", "region", "transform"), &Navigation2DServer::region_set_transform);
ClassDB::bind_method(D_METHOD("region_set_navpoly", "region", "nav_poly"), &Navigation2DServer::region_set_navpoly);
ClassDB::bind_method(D_METHOD("agent_create"), &Navigation2DServer::agent_create);
ClassDB::bind_method(D_METHOD("agent_set_map", "agent", "map"), &Navigation2DServer::agent_set_map);
ClassDB::bind_method(D_METHOD("agent_set_neighbor_dist", "agent", "dist"), &Navigation2DServer::agent_set_neighbor_dist);
ClassDB::bind_method(D_METHOD("agent_set_max_neighbors", "agent", "count"), &Navigation2DServer::agent_set_max_neighbors);
ClassDB::bind_method(D_METHOD("agent_set_time_horizon", "agent", "time"), &Navigation2DServer::agent_set_time_horizon);
ClassDB::bind_method(D_METHOD("agent_set_radius", "agent", "radius"), &Navigation2DServer::agent_set_radius);
ClassDB::bind_method(D_METHOD("agent_set_max_speed", "agent", "max_speed"), &Navigation2DServer::agent_set_max_speed);
ClassDB::bind_method(D_METHOD("agent_set_velocity", "agent", "velocity"), &Navigation2DServer::agent_set_velocity);
ClassDB::bind_method(D_METHOD("agent_set_velocity_target", "agent", "target_velocity"), &Navigation2DServer::agent_set_target_velocity);
ClassDB::bind_method(D_METHOD("agent_set_position", "agent", "position"), &Navigation2DServer::agent_set_position);
ClassDB::bind_method(D_METHOD("agent_is_map_changed", "agent"), &Navigation2DServer::agent_is_map_changed);
ClassDB::bind_method(D_METHOD("agent_set_callback", "agent", "receiver", "method", "userdata"), &Navigation2DServer::agent_set_callback, DEFVAL(Variant()));
ClassDB::bind_method(D_METHOD("free", "object"), &Navigation2DServer::free);
}
Navigation2DServer::Navigation2DServer() {
singleton = this;
}
Navigation2DServer::~Navigation2DServer() {
singleton = NULL;
}
RID FORWARD_0_C(map_create);
void FORWARD_2_C(map_set_active, RID, p_map, bool, p_active, rid_to_rid, bool_to_bool);
bool FORWARD_1_C(map_is_active, RID, p_map, rid_to_rid);
void FORWARD_2_C(map_set_cell_size, RID, p_map, real_t, p_cell_size, rid_to_rid, real_to_real);
real_t FORWARD_1_C(map_get_cell_size, RID, p_map, rid_to_rid);
void FORWARD_2_C(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin, rid_to_rid, real_to_real);
real_t FORWARD_1_C(map_get_edge_connection_margin, RID, p_map, rid_to_rid);
Vector<Vector2> FORWARD_4_R_C(vector_v3_to_v2, map_get_path, RID, p_map, Vector2, p_origin, Vector2, p_destination, bool, p_optimize, rid_to_rid, v2_to_v3, v2_to_v3, bool_to_bool);
RID FORWARD_0_C(region_create);
void FORWARD_2_C(region_set_map, RID, p_region, RID, p_map, rid_to_rid, rid_to_rid);
void FORWARD_2_C(region_set_transform, RID, p_region, Transform2D, p_transform, rid_to_rid, trf2_to_trf3);
void Navigation2DServer::region_set_navpoly(RID p_region, Ref<NavigationPolygon> p_nav_mesh) const {
NavigationServer::get_singleton()->region_set_navmesh(p_region, poly_to_mesh(p_nav_mesh));
}
RID Navigation2DServer::agent_create() const {
RID agent = NavigationServer::get_singleton()->agent_create();
NavigationServer::get_singleton()->agent_set_ignore_y(agent, true);
return agent;
}
void FORWARD_2_C(agent_set_map, RID, p_agent, RID, p_map, rid_to_rid, rid_to_rid);
void FORWARD_2_C(agent_set_neighbor_dist, RID, p_agent, real_t, p_dist, rid_to_rid, real_to_real);
void FORWARD_2_C(agent_set_max_neighbors, RID, p_agent, int, p_count, rid_to_rid, int_to_int);
void FORWARD_2_C(agent_set_time_horizon, RID, p_agent, real_t, p_time, rid_to_rid, real_to_real);
void FORWARD_2_C(agent_set_radius, RID, p_agent, real_t, p_radius, rid_to_rid, real_to_real);
void FORWARD_2_C(agent_set_max_speed, RID, p_agent, real_t, p_max_speed, rid_to_rid, real_to_real);
void FORWARD_2_C(agent_set_velocity, RID, p_agent, Vector2, p_velocity, rid_to_rid, v2_to_v3);
void FORWARD_2_C(agent_set_target_velocity, RID, p_agent, Vector2, p_velocity, rid_to_rid, v2_to_v3);
void FORWARD_2_C(agent_set_position, RID, p_agent, Vector2, p_position, rid_to_rid, v2_to_v3);
void FORWARD_2_C(agent_set_ignore_y, RID, p_agent, bool, p_ignore, rid_to_rid, bool_to_bool);
bool FORWARD_1_C(agent_is_map_changed, RID, p_agent, rid_to_rid);
void FORWARD_4_C(agent_set_callback, RID, p_agent, Object *, p_receiver, StringName, p_method, Variant, p_udata, rid_to_rid, obj_to_obj, sn_to_sn, var_to_var);
void FORWARD_1_C(free, RID, p_object, rid_to_rid);

160
servers/navigation_2d_server.h Normal file
View File

@ -0,0 +1,160 @@
/*************************************************************************/
/* navigation_2d_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. */
/*************************************************************************/
/**
@author AndreaCatania
*/
#ifndef NAVIGATION_2D_SERVER_H
#define NAVIGATION_2D_SERVER_H
#include "core/object.h"
#include "core/rid.h"
#include "scene/2d/navigation_polygon.h"
// This server exposes the 3D `NavigationServer` features in the 2D world.
class Navigation2DServer : public Object {
GDCLASS(Navigation2DServer, Object);
static Navigation2DServer *singleton;
protected:
static void _bind_methods();
public:
/// Thread safe, can be used accross many threads.
static const Navigation2DServer *get_singleton() { return singleton; }
/// MUST be used in single thread!
static Navigation2DServer *get_singleton_mut() { return singleton; }
/// Create a new map.
virtual RID map_create() const;
/// Set map active.
virtual void map_set_active(RID p_map, bool p_active) const;
/// Returns true if the map is active.
virtual bool map_is_active(RID p_map) const;
/// Set the map cell size used to weld the navigation mesh polygons.
virtual void map_set_cell_size(RID p_map, real_t p_cell_size) const;
/// Returns the map cell size.
virtual real_t map_get_cell_size(RID p_map) const;
/// Set the map edge connection margin used to weld the compatible region edges.
virtual void map_set_edge_connection_margin(RID p_map, real_t p_connection_margin) const;
/// Returns the edge connection margin of this map.
virtual real_t map_get_edge_connection_margin(RID p_map) const;
/// Returns the navigation path to reach the destination from the origin.
virtual Vector<Vector2> map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize) const;
/// Creates a new region.
virtual RID region_create() const;
/// Set the map of this region.
virtual void region_set_map(RID p_region, RID p_map) const;
/// Set the global transformation of this region.
virtual void region_set_transform(RID p_region, Transform2D p_transform) const;
/// Set the navigation poly of this region.
virtual void region_set_navpoly(RID p_region, Ref<NavigationPolygon> p_nav_mesh) const;
/// Creates the agent.
virtual RID agent_create() const;
/// Put the agent in the map.
virtual void agent_set_map(RID p_agent, RID p_map) const;
/// The maximum distance (center point to
/// center point) to other agents this agent
/// takes into account in the navigation. The
/// larger this number, the longer the running
/// time of the simulation. If the number is too
/// low, the simulation will not be safe.
/// Must be non-negative.
virtual void agent_set_neighbor_dist(RID p_agent, real_t p_dist) const;
/// The maximum number of other agents this
/// agent takes into account in the navigation.
/// The larger this number, the longer the
/// running time of the simulation. If the
/// number is too low, the simulation will not
/// be safe.
virtual void agent_set_max_neighbors(RID p_agent, int p_count) const;
/// The minimal amount of time for which this
/// agent's velocities that are computed by the
/// simulation are safe with respect to other
/// agents. The larger this number, the sooner
/// this agent will respond to the presence of
/// other agents, but the less freedom this
/// agent has in choosing its velocities.
/// Must be positive.
virtual void agent_set_time_horizon(RID p_agent, real_t p_time) const;
/// The radius of this agent.
/// Must be non-negative.
virtual void agent_set_radius(RID p_agent, real_t p_radius) const;
/// The maximum speed of this agent.
/// Must be non-negative.
virtual void agent_set_max_speed(RID p_agent, real_t p_max_speed) const;
/// Current velocity of the agent
virtual void agent_set_velocity(RID p_agent, Vector2 p_velocity) const;
/// The new target velocity.
virtual void agent_set_target_velocity(RID p_agent, Vector2 p_velocity) const;
/// Position of the agent in world space.
virtual void agent_set_position(RID p_agent, Vector2 p_position) const;
/// Agent ignore the Y axis and avoid collisions by moving only on the horizontal plane
virtual void agent_set_ignore_y(RID p_agent, bool p_ignore) const;
/// Returns true if the map got changed the previous frame.
virtual bool agent_is_map_changed(RID p_agent) const;
/// Callback called at the end of the RVO process
virtual void agent_set_callback(RID p_agent, Object *p_receiver, StringName p_method, Variant p_udata = Variant()) const;
/// Destroy the `RID`
virtual void free(RID p_object) const;
Navigation2DServer();
virtual ~Navigation2DServer();
};
#endif

103
servers/navigation_server.cpp Normal file
View File

@ -0,0 +1,103 @@
/*************************************************************************/
/* 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. */
/*************************************************************************/
/**
@author AndreaCatania
*/
#include "navigation_server.h"
NavigationServer *NavigationServer::singleton = NULL;
void NavigationServer::_bind_methods() {
ClassDB::bind_method(D_METHOD("map_create"), &NavigationServer::map_create);
ClassDB::bind_method(D_METHOD("map_set_active", "map", "active"), &NavigationServer::map_set_active);
ClassDB::bind_method(D_METHOD("map_is_active", "nap"), &NavigationServer::map_is_active);
ClassDB::bind_method(D_METHOD("map_set_up", "map", "up"), &NavigationServer::map_set_up);
ClassDB::bind_method(D_METHOD("map_get_up", "map"), &NavigationServer::map_get_up);
ClassDB::bind_method(D_METHOD("map_set_cell_size", "map", "cell_size"), &NavigationServer::map_set_cell_size);
ClassDB::bind_method(D_METHOD("map_get_cell_size", "map"), &NavigationServer::map_get_cell_size);
ClassDB::bind_method(D_METHOD("map_set_edge_connection_margin", "map", "margin"), &NavigationServer::map_set_edge_connection_margin);
ClassDB::bind_method(D_METHOD("map_get_edge_connection_margin", "map"), &NavigationServer::map_get_edge_connection_margin);
ClassDB::bind_method(D_METHOD("map_get_path", "map", "origin", "destination", "optimize"), &NavigationServer::map_get_path);
ClassDB::bind_method(D_METHOD("region_create"), &NavigationServer::region_create);
ClassDB::bind_method(D_METHOD("region_set_map", "region", "map"), &NavigationServer::region_set_map);
ClassDB::bind_method(D_METHOD("region_set_transform", "region", "transform"), &NavigationServer::region_set_transform);
ClassDB::bind_method(D_METHOD("region_set_navmesh", "region", "nav_mesh"), &NavigationServer::region_set_navmesh);
ClassDB::bind_method(D_METHOD("region_bake_navmesh", "mesh", "node"), &NavigationServer::region_bake_navmesh);
ClassDB::bind_method(D_METHOD("agent_create"), &NavigationServer::agent_create);
ClassDB::bind_method(D_METHOD("agent_set_map", "agent", "map"), &NavigationServer::agent_set_map);
ClassDB::bind_method(D_METHOD("agent_set_neighbor_dist", "agent", "dist"), &NavigationServer::agent_set_neighbor_dist);
ClassDB::bind_method(D_METHOD("agent_set_max_neighbors", "agent", "count"), &NavigationServer::agent_set_max_neighbors);
ClassDB::bind_method(D_METHOD("agent_set_time_horizon", "agent", "time"), &NavigationServer::agent_set_time_horizon);
ClassDB::bind_method(D_METHOD("agent_set_radius", "agent", "radius"), &NavigationServer::agent_set_radius);
ClassDB::bind_method(D_METHOD("agent_set_max_speed", "agent", "max_speed"), &NavigationServer::agent_set_max_speed);
ClassDB::bind_method(D_METHOD("agent_set_velocity", "agent", "velocity"), &NavigationServer::agent_set_velocity);
ClassDB::bind_method(D_METHOD("agent_set_velocity_target", "agent", "target_velocity"), &NavigationServer::agent_set_target_velocity);
ClassDB::bind_method(D_METHOD("agent_set_position", "agent", "position"), &NavigationServer::agent_set_position);
ClassDB::bind_method(D_METHOD("agent_is_map_changed", "agent"), &NavigationServer::agent_is_map_changed);
ClassDB::bind_method(D_METHOD("agent_set_callback", "agent", "receiver", "method", "userdata"), &NavigationServer::agent_set_callback, DEFVAL(Variant()));
ClassDB::bind_method(D_METHOD("free", "object"), &NavigationServer::free);
ClassDB::bind_method(D_METHOD("set_active", "active"), &NavigationServer::set_active);
ClassDB::bind_method(D_METHOD("step", "delta_time"), &NavigationServer::step);
}
const NavigationServer *NavigationServer::get_singleton() {
return singleton;
}
NavigationServer *NavigationServer::get_singleton_mut() {
return singleton;
}
NavigationServer::NavigationServer() {
ERR_FAIL_COND(singleton != NULL);
singleton = this;
}
NavigationServer::~NavigationServer() {
singleton = NULL;
}
NavigationServerCallback NavigationServerManager::create_callback = NULL;
void NavigationServerManager::set_default_server(NavigationServerCallback p_callback) {
create_callback = p_callback;
}
NavigationServer *NavigationServerManager::new_default_server() {
ERR_FAIL_COND_V(create_callback == NULL, NULL);
return create_callback();
}

192
servers/navigation_server.h Normal file
View File

@ -0,0 +1,192 @@
/*************************************************************************/
/* 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. */
/*************************************************************************/
/**
@author AndreaCatania
*/
#ifndef NAVIGATION_SERVER_H
#define NAVIGATION_SERVER_H
#include "core/object.h"
#include "core/rid.h"
#include "scene/3d/navigation_mesh_instance.h"
/// This server uses the concept of internal mutability.
/// All the constant functions can be called in multithread because internally
/// the server takes care to schedule the functions access.
///
/// Note: All the `set` functions are commands executed during the `sync` phase,
/// don't expect that a change is immediately propagated.
class NavigationServer : public Object {
GDCLASS(NavigationServer, Object);
static NavigationServer *singleton;
protected:
static void _bind_methods();
public:
/// Thread safe, can be used accross many threads.
static const NavigationServer *get_singleton();
/// MUST be used in single thread!
static NavigationServer *get_singleton_mut();
/// Create a new map.
virtual RID map_create() const = 0;
/// Set map active.
virtual void map_set_active(RID p_map, bool p_active) const = 0;
/// Returns true if the map is active.
virtual bool map_is_active(RID p_map) const = 0;
/// Set the map UP direction.
virtual void map_set_up(RID p_map, Vector3 p_up) const = 0;
/// Returns the map UP direction.
virtual Vector3 map_get_up(RID p_map) const = 0;
/// Set the map cell size used to weld the navigation mesh polygons.
virtual void map_set_cell_size(RID p_map, real_t p_cell_size) const = 0;
/// Returns the map cell size.
virtual real_t map_get_cell_size(RID p_map) const = 0;
/// Set the map edge connection margin used to weld the compatible region edges.
virtual void map_set_edge_connection_margin(RID p_map, real_t p_connection_margin) const = 0;
/// Returns the edge connection margin of this map.
virtual real_t map_get_edge_connection_margin(RID p_map) const = 0;
/// Returns the navigation path to reach the destination from the origin.
virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) const = 0;
/// Creates a new region.
virtual RID region_create() const = 0;
/// Set the map of this region.
virtual void region_set_map(RID p_region, RID p_map) const = 0;
/// Set the global transformation of this region.
virtual void region_set_transform(RID p_region, Transform p_transform) const = 0;
/// Set the navigation mesh of this region.
virtual void region_set_navmesh(RID p_region, Ref<NavigationMesh> p_nav_mesh) const = 0;
/// Bake the navigation mesh
virtual void region_bake_navmesh(Ref<NavigationMesh> r_mesh, Node *p_node) const = 0;
/// Creates the agent.
virtual RID agent_create() const = 0;
/// Put the agent in the map.
virtual void agent_set_map(RID p_agent, RID p_map) const = 0;
/// The maximum distance (center point to
/// center point) to other agents this agent
/// takes into account in the navigation. The
/// larger this number, the longer the running
/// time of the simulation. If the number is too
/// low, the simulation will not be safe.
/// Must be non-negative.
virtual void agent_set_neighbor_dist(RID p_agent, real_t p_dist) const = 0;
/// The maximum number of other agents this
/// agent takes into account in the navigation.
/// The larger this number, the longer the
/// running time of the simulation. If the
/// number is too low, the simulation will not
/// be safe.
virtual void agent_set_max_neighbors(RID p_agent, int p_count) const = 0;
/// The minimal amount of time for which this
/// agent's velocities that are computed by the
/// simulation are safe with respect to other
/// agents. The larger this number, the sooner
/// this agent will respond to the presence of
/// other agents, but the less freedom this
/// agent has in choosing its velocities.
/// Must be positive.
virtual void agent_set_time_horizon(RID p_agent, real_t p_time) const = 0;
/// The radius of this agent.
/// Must be non-negative.
virtual void agent_set_radius(RID p_agent, real_t p_radius) const = 0;
/// The maximum speed of this agent.
/// Must be non-negative.
virtual void agent_set_max_speed(RID p_agent, real_t p_max_speed) const = 0;
/// Current velocity of the agent
virtual void agent_set_velocity(RID p_agent, Vector3 p_velocity) const = 0;
/// The new target velocity.
virtual void agent_set_target_velocity(RID p_agent, Vector3 p_velocity) const = 0;
/// Position of the agent in world space.
virtual void agent_set_position(RID p_agent, Vector3 p_position) const = 0;
/// Agent ignore the Y axis and avoid collisions by moving only on the horizontal plane
virtual void agent_set_ignore_y(RID p_agent, bool p_ignore) const = 0;
/// Returns true if the map got changed the previous frame.
virtual bool agent_is_map_changed(RID p_agent) const = 0;
/// Callback called at the end of the RVO process
virtual void agent_set_callback(RID p_agent, Object *p_receiver, StringName p_method, Variant p_udata = Variant()) const = 0;
/// Destroy the `RID`
virtual void free(RID p_object) const = 0;
/// Control activation of this server.
virtual void set_active(bool p_active) const = 0;
/// Step the server
/// NOTE: This function is not Threadsafe and MUST be called in single thread.
virtual void step(real_t delta_time) = 0;
NavigationServer();
virtual ~NavigationServer();
};
typedef NavigationServer *(*NavigationServerCallback)();
/// Manager used for the server singleton registration
class NavigationServerManager {
static NavigationServerCallback create_callback;
public:
static void set_default_server(NavigationServerCallback p_callback);
static NavigationServer *new_default_server();
};
#endif

4
servers/register_server_types.cpp
View File

@ -56,6 +56,8 @@
#include "audio_server.h"
#include "camera/camera_feed.h"
#include "camera_server.h"
#include "navigation_2d_server.h"
#include "navigation_server.h"
#include "physics/physics_server_sw.h"
#include "physics_2d/physics_2d_server_sw.h"
#include "physics_2d/physics_2d_server_wrap_mt.h"
@ -212,6 +214,8 @@ void register_server_singletons() {
Engine::get_singleton()->add_singleton(Engine::Singleton("AudioServer", AudioServer::get_singleton()));
Engine::get_singleton()->add_singleton(Engine::Singleton("PhysicsServer", PhysicsServer::get_singleton()));
Engine::get_singleton()->add_singleton(Engine::Singleton("Physics2DServer", Physics2DServer::get_singleton()));
Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationServer", NavigationServer::get_singleton_mut()));
Engine::get_singleton()->add_singleton(Engine::Singleton("Navigation2DServer", Navigation2DServer::get_singleton_mut()));
Engine::get_singleton()->add_singleton(Engine::Singleton("ARVRServer", ARVRServer::get_singleton()));
Engine::get_singleton()->add_singleton(Engine::Singleton("CameraServer", CameraServer::get_singleton()));
}

14
thirdparty/README.md vendored
View File

@ -441,6 +441,20 @@ Files extracted from upstream source:
- License.txt
## Rvo2-3D
- Upstream: http://gamma.cs.unc.edu/RVO2/
- version: 1.0.1
- License: Apache 2.0
Files extracted from upstream source:
- All .cpp and .h files in the `src/` folder except for RVOSimulator.cpp and RVOSimulator.h
- LICENSE
- README
Important: Some files have Godot-made changes; so to enrich the features originally proposed by this library and better integrate this library with Godot. Please check the file to know what's new.
## squish
- Upstream: https://sourceforge.net/projects/libsquish

202
thirdparty/rvo2/LICENSE vendored Normal file
View File

@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

32
thirdparty/rvo2/README.md vendored Normal file
View File

@ -0,0 +1,32 @@
Optimal Reciprocal Collision Avoidance in Three Dimensions
==========================================================
<http://gamma.cs.unc.edu/RVO2/>
[![Build Status](https://travis-ci.org/snape/RVO2-3D.png?branch=master)](https://travis-ci.org/snape/RVO2-3D)
[![Build status](https://ci.appveyor.com/api/projects/status/ov8ec3igv588wpx7/branch/master?svg=true)](https://ci.appveyor.com/project/snape/rvo2-3d)
Copyright 2008 University of North Carolina at Chapel Hill
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
<http://www.apache.org/licenses/LICENSE-2.0>
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
Please send all bug reports to [geom@cs.unc.edu](mailto:geom@cs.unc.edu).
The authors may be contacted via:
Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, and Dinesh Manocha
Dept. of Computer Science
201 S. Columbia St.
Frederick P. Brooks, Jr. Computer Science Bldg.
Chapel Hill, N.C. 27599-3175
United States of America

71
thirdparty/rvo2/src/API.h vendored Normal file
View File

@ -0,0 +1,71 @@
/*
* API.h
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
/**
* \file API.h
* \brief Contains definitions related to Microsoft Windows.
*/
#ifndef RVO_API_H_
#define RVO_API_H_
#ifdef _WIN32
#include <SDKDDKVer.h>
#define WIN32_LEAN_AND_MEAN
#define NOCOMM
#define NOIMAGE
#define NOIME
#define NOKANJI
#define NOMCX
#ifndef NOMINMAX
#define NOMINMAX
#endif
#define NOPROXYSTUB
#define NOSERVICE
#define NOSOUND
#define NOTAPE
#define NORPC
#define _USE_MATH_DEFINES
#include <windows.h>
#undef CONNECT_DEFERRED // Avoid collision with the Godot Object class
#undef CreateDialog // Avoid collision with the Godot CreateDialog class
#endif
#ifdef RVO_EXPORTS
#define RVO_API __declspec(dllexport)
#elif defined(RVO_IMPORTS)
#define RVO_API __declspec(dllimport)
#else
#define RVO_API
#endif
#endif /* RVO_API_H_ */

425
thirdparty/rvo2/src/Agent.cpp vendored Normal file
View File

@ -0,0 +1,425 @@
/*
* Agent.cpp
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#include "Agent.h"
#include <algorithm>
#include <cmath>
#include "Definitions.h"
#include "KdTree.h"
namespace RVO {
/**
* \brief A sufficiently small positive number.
*/
const float RVO_EPSILON = 0.00001f;
/**
* \brief Defines a directed line.
*/
class Line {
public:
/**
* \brief The direction of the directed line.
*/
Vector3 direction;
/**
* \brief A point on the directed line.
*/
Vector3 point;
};
/**
* \brief Solves a one-dimensional linear program on a specified line subject to linear constraints defined by planes and a spherical constraint.
* \param planes Planes defining the linear constraints.
* \param planeNo The plane on which the line lies.
* \param line The line on which the 1-d linear program is solved
* \param radius The radius of the spherical constraint.
* \param optVelocity The optimization velocity.
* \param directionOpt True if the direction should be optimized.
* \param result A reference to the result of the linear program.
* \return True if successful.
*/
bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
/**
* \brief Solves a two-dimensional linear program on a specified plane subject to linear constraints defined by planes and a spherical constraint.
* \param planes Planes defining the linear constraints.
* \param planeNo The plane on which the 2-d linear program is solved
* \param radius The radius of the spherical constraint.
* \param optVelocity The optimization velocity.
* \param directionOpt True if the direction should be optimized.
* \param result A reference to the result of the linear program.
* \return True if successful.
*/
bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
/**
* \brief Solves a three-dimensional linear program subject to linear constraints defined by planes and a spherical constraint.
* \param planes Planes defining the linear constraints.
* \param radius The radius of the spherical constraint.
* \param optVelocity The optimization velocity.
* \param directionOpt True if the direction should be optimized.
* \param result A reference to the result of the linear program.
* \return The number of the plane it fails on, and the number of planes if successful.
*/
size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
/**
* \brief Solves a four-dimensional linear program subject to linear constraints defined by planes and a spherical constraint.
* \param planes Planes defining the linear constraints.
* \param beginPlane The plane on which the 3-d linear program failed.
* \param radius The radius of the spherical constraint.
* \param result A reference to the result of the linear program.
*/
void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result);
Agent::Agent() :
id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), ignore_y_(false) {}
void Agent::computeNeighbors(KdTree *kdTree_) {
agentNeighbors_.clear();
if (maxNeighbors_ > 0) {
kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_);
}
}
#define ABS(m_v) (((m_v) < 0) ? (-(m_v)) : (m_v))
void Agent::computeNewVelocity(float timeStep) {
orcaPlanes_.clear();
const float invTimeHorizon = 1.0f / timeHorizon_;
/* Create agent ORCA planes. */
for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
const Agent *const other = agentNeighbors_[i].second;
Vector3 relativePosition = other->position_ - position_;
Vector3 relativeVelocity = velocity_ - other->velocity_;
const float combinedRadius = radius_ + other->radius_;
// This is a Godot feature that allow the agents to avoid the collision
// by moving only on the horizontal plane relative to the player velocity.
if (ignore_y_) {
// Skip if these are in two different heights
if (ABS(relativePosition[1]) > combinedRadius * 2) {
continue;
}
relativePosition[1] = 0;
relativeVelocity[1] = 0;
}
const float distSq = absSq(relativePosition);
const float combinedRadiusSq = sqr(combinedRadius);
Plane plane;
Vector3 u;
if (distSq > combinedRadiusSq) {
/* No collision. */
const Vector3 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
const float wLengthSq = absSq(w);
const float dotProduct = w * relativePosition;
if (dotProduct < 0.0f && sqr(dotProduct) > combinedRadiusSq * wLengthSq) {
/* Project on cut-off circle. */
const float wLength = std::sqrt(wLengthSq);
const Vector3 unitW = w / wLength;
plane.normal = unitW;
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
} else {
/* Project on cone. */
const float a = distSq;
const float b = relativePosition * relativeVelocity;
const float c = absSq(relativeVelocity) - absSq(cross(relativePosition, relativeVelocity)) / (distSq - combinedRadiusSq);
const float t = (b + std::sqrt(sqr(b) - a * c)) / a;
const Vector3 w = relativeVelocity - t * relativePosition;
const float wLength = abs(w);
const Vector3 unitW = w / wLength;
plane.normal = unitW;
u = (combinedRadius * t - wLength) * unitW;
}
} else {
/* Collision. */
const float invTimeStep = 1.0f / timeStep;
const Vector3 w = relativeVelocity - invTimeStep * relativePosition;
const float wLength = abs(w);
const Vector3 unitW = w / wLength;
plane.normal = unitW;
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
plane.point = velocity_ + 0.5f * u;
orcaPlanes_.push_back(plane);
}
const size_t planeFail = linearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_);
if (planeFail < orcaPlanes_.size()) {
linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_);
}
if (ignore_y_) {
// Not 100% necessary, but better to have.
newVelocity_[1] = prefVelocity_[1];
}
}
void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq) {
if (this != agent) {
const float distSq = absSq(position_ - agent->position_);
if (distSq < rangeSq) {
if (agentNeighbors_.size() < maxNeighbors_) {
agentNeighbors_.push_back(std::make_pair(distSq, agent));
}
size_t i = agentNeighbors_.size() - 1;
while (i != 0 && distSq < agentNeighbors_[i - 1].first) {
agentNeighbors_[i] = agentNeighbors_[i - 1];
--i;
}
agentNeighbors_[i] = std::make_pair(distSq, agent);
if (agentNeighbors_.size() == maxNeighbors_) {
rangeSq = agentNeighbors_.back().first;
}
}
}
}
bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result) {
const float dotProduct = line.point * line.direction;
const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(line.point);
if (discriminant < 0.0f) {
/* Max speed sphere fully invalidates line. */
return false;
}
const float sqrtDiscriminant = std::sqrt(discriminant);
float tLeft = -dotProduct - sqrtDiscriminant;
float tRight = -dotProduct + sqrtDiscriminant;
for (size_t i = 0; i < planeNo; ++i) {
const float numerator = (planes[i].point - line.point) * planes[i].normal;
const float denominator = line.direction * planes[i].normal;
if (sqr(denominator) <= RVO_EPSILON) {
/* Lines line is (almost) parallel to plane i. */
if (numerator > 0.0f) {
return false;
} else {
continue;
}
}
const float t = numerator / denominator;
if (denominator >= 0.0f) {
/* Plane i bounds line on the left. */
tLeft = std::max(tLeft, t);
} else {
/* Plane i bounds line on the right. */
tRight = std::min(tRight, t);
}
if (tLeft > tRight) {
return false;
}
}
if (directionOpt) {
/* Optimize direction. */
if (optVelocity * line.direction > 0.0f) {
/* Take right extreme. */
result = line.point + tRight * line.direction;
} else {
/* Take left extreme. */
result = line.point + tLeft * line.direction;
}
} else {
/* Optimize closest point. */
const float t = line.direction * (optVelocity - line.point);
if (t < tLeft) {
result = line.point + tLeft * line.direction;
} else if (t > tRight) {
result = line.point + tRight * line.direction;
} else {
result = line.point + t * line.direction;
}
}
return true;
}
bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result) {
const float planeDist = planes[planeNo].point * planes[planeNo].normal;
const float planeDistSq = sqr(planeDist);
const float radiusSq = sqr(radius);
if (planeDistSq > radiusSq) {
/* Max speed sphere fully invalidates plane planeNo. */
return false;
}
const float planeRadiusSq = radiusSq - planeDistSq;
const Vector3 planeCenter = planeDist * planes[planeNo].normal;
if (directionOpt) {
/* Project direction optVelocity on plane planeNo. */
const Vector3 planeOptVelocity = optVelocity - (optVelocity * planes[planeNo].normal) * planes[planeNo].normal;
const float planeOptVelocityLengthSq = absSq(planeOptVelocity);
if (planeOptVelocityLengthSq <= RVO_EPSILON) {
result = planeCenter;
} else {
result = planeCenter + std::sqrt(planeRadiusSq / planeOptVelocityLengthSq) * planeOptVelocity;
}
} else {
/* Project point optVelocity on plane planeNo. */
result = optVelocity + ((planes[planeNo].point - optVelocity) * planes[planeNo].normal) * planes[planeNo].normal;
/* If outside planeCircle, project on planeCircle. */
if (absSq(result) > radiusSq) {
const Vector3 planeResult = result - planeCenter;
const float planeResultLengthSq = absSq(planeResult);
result = planeCenter + std::sqrt(planeRadiusSq / planeResultLengthSq) * planeResult;
}
}
for (size_t i = 0; i < planeNo; ++i) {
if (planes[i].normal * (planes[i].point - result) > 0.0f) {
/* Result does not satisfy constraint i. Compute new optimal result. */
/* Compute intersection line of plane i and plane planeNo. */
Vector3 crossProduct = cross(planes[i].normal, planes[planeNo].normal);
if (absSq(crossProduct) <= RVO_EPSILON) {
/* Planes planeNo and i are (almost) parallel, and plane i fully invalidates plane planeNo. */
return false;
}
Line line;
line.direction = normalize(crossProduct);
const Vector3 lineNormal = cross(line.direction, planes[planeNo].normal);
line.point = planes[planeNo].point + (((planes[i].point - planes[planeNo].point) * planes[i].normal) / (lineNormal * planes[i].normal)) * lineNormal;
if (!linearProgram1(planes, i, line, radius, optVelocity, directionOpt, result)) {
return false;
}
}
}
return true;
}
size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result) {
if (directionOpt) {
/* Optimize direction. Note that the optimization velocity is of unit length in this case. */
result = optVelocity * radius;
} else if (absSq(optVelocity) > sqr(radius)) {
/* Optimize closest point and outside circle. */
result = normalize(optVelocity) * radius;
} else {
/* Optimize closest point and inside circle. */
result = optVelocity;
}
for (size_t i = 0; i < planes.size(); ++i) {
if (planes[i].normal * (planes[i].point - result) > 0.0f) {
/* Result does not satisfy constraint i. Compute new optimal result. */
const Vector3 tempResult = result;
if (!linearProgram2(planes, i, radius, optVelocity, directionOpt, result)) {
result = tempResult;
return i;
}
}
}
return planes.size();
}
void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result) {
float distance = 0.0f;
for (size_t i = beginPlane; i < planes.size(); ++i) {
if (planes[i].normal * (planes[i].point - result) > distance) {
/* Result does not satisfy constraint of plane i. */
std::vector<Plane> projPlanes;
for (size_t j = 0; j < i; ++j) {
Plane plane;
const Vector3 crossProduct = cross(planes[j].normal, planes[i].normal);
if (absSq(crossProduct) <= RVO_EPSILON) {
/* Plane i and plane j are (almost) parallel. */
if (planes[i].normal * planes[j].normal > 0.0f) {
/* Plane i and plane j point in the same direction. */
continue;
} else {
/* Plane i and plane j point in opposite direction. */
plane.point = 0.5f * (planes[i].point + planes[j].point);
}
} else {
/* Plane.point is point on line of intersection between plane i and plane j. */
const Vector3 lineNormal = cross(crossProduct, planes[i].normal);
plane.point = planes[i].point + (((planes[j].point - planes[i].point) * planes[j].normal) / (lineNormal * planes[j].normal)) * lineNormal;
}
plane.normal = normalize(planes[j].normal - planes[i].normal);
projPlanes.push_back(plane);
}
const Vector3 tempResult = result;
if (linearProgram3(projPlanes, radius, planes[i].normal, true, result) < projPlanes.size()) {
/* This should in principle not happen. The result is by definition already in the feasible region of this linear program. If it fails, it is due to small floating point error, and the current result is kept. */
result = tempResult;
}
distance = planes[i].normal * (planes[i].point - result);
}
}
}
} // namespace RVO

121
thirdparty/rvo2/src/Agent.h vendored Normal file
View File

@ -0,0 +1,121 @@
/*
* Agent.h
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
/**
* \file Agent.h
* \brief Contains the Agent class.
*/
#ifndef RVO_AGENT_H_
#define RVO_AGENT_H_
#include "API.h"
#include <cstddef>
#include <utility>
#include <vector>
#include "Vector3.h"
// Note: Slightly modified to work better in Godot.
// - The agent can be created by anyone.
// - The simulator pointer is removed.
// - The update function is removed.
// - The compute velocity function now need the timeStep.
// - Moved the `Plane` class here.
// - Added a new parameter `ignore_y_` in the `Agent`. This parameter is used to control a godot feature that allows to avoid collisions by moving on the horizontal plane.
namespace RVO {
/**
* \brief Defines a plane.
*/
class Plane {
public:
/**
* \brief A point on the plane.
*/
Vector3 point;
/**
* \brief The normal to the plane.
*/
Vector3 normal;
};
/**
* \brief Defines an agent in the simulation.
*/
class Agent {
public:
/**
* \brief Constructs an agent instance.
* \param sim The simulator instance.
*/
explicit Agent();
/**
* \brief Computes the neighbors of this agent.
*/
void computeNeighbors(class KdTree *kdTree_);
/**
* \brief Computes the new velocity of this agent.
*/
void computeNewVelocity(float timeStep);
/**
* \brief Inserts an agent neighbor into the set of neighbors of this agent.
* \param agent A pointer to the agent to be inserted.
* \param rangeSq The squared range around this agent.
*/
void insertAgentNeighbor(const Agent *agent, float &rangeSq);
Vector3 newVelocity_;
Vector3 position_;
Vector3 prefVelocity_;
Vector3 velocity_;
size_t id_;
size_t maxNeighbors_;
float maxSpeed_;
float neighborDist_;
float radius_;
float timeHorizon_;
std::vector<std::pair<float, const Agent *> > agentNeighbors_;
std::vector<Plane> orcaPlanes_;
/// This is a godot feature that allows the Agent to avoid collision by mooving
/// on the horizontal plane.
bool ignore_y_;
friend class KdTree;
};
} // namespace RVO
#endif /* RVO_AGENT_H_ */

55
thirdparty/rvo2/src/Definitions.h vendored Normal file
View File

@ -0,0 +1,55 @@
/*
* Definitions.h
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
/**
* \file Definitions.h
* \brief Contains functions and constants used in multiple classes.
*/
#ifndef RVO_DEFINITIONS_H_
#define RVO_DEFINITIONS_H_
#include "API.h"
namespace RVO {
/**
* \brief Computes the square of a float.
* \param scalar The float to be squared.
* \return The square of the float.
*/
inline float sqr(float scalar)
{
return scalar * scalar;
}
}
#endif /* RVO_DEFINITIONS_H_ */

152
thirdparty/rvo2/src/KdTree.cpp vendored Normal file
View File

@ -0,0 +1,152 @@
/*
* KdTree.cpp
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#include "KdTree.h"
#include <algorithm>
#include "Agent.h"
#include "Definitions.h"
namespace RVO {
const size_t RVO_MAX_LEAF_SIZE = 10;
KdTree::KdTree() {}
void KdTree::buildAgentTree(std::vector<Agent *> agents) {
agents_.swap(agents);
if (!agents_.empty()) {
agentTree_.resize(2 * agents_.size() - 1);
buildAgentTreeRecursive(0, agents_.size(), 0);
}
}
void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) {
agentTree_[node].begin = begin;
agentTree_[node].end = end;
agentTree_[node].minCoord = agents_[begin]->position_;
agentTree_[node].maxCoord = agents_[begin]->position_;
for (size_t i = begin + 1; i < end; ++i) {
agentTree_[node].maxCoord[0] = std::max(agentTree_[node].maxCoord[0], agents_[i]->position_.x());
agentTree_[node].minCoord[0] = std::min(agentTree_[node].minCoord[0], agents_[i]->position_.x());
agentTree_[node].maxCoord[1] = std::max(agentTree_[node].maxCoord[1], agents_[i]->position_.y());
agentTree_[node].minCoord[1] = std::min(agentTree_[node].minCoord[1], agents_[i]->position_.y());
agentTree_[node].maxCoord[2] = std::max(agentTree_[node].maxCoord[2], agents_[i]->position_.z());
agentTree_[node].minCoord[2] = std::min(agentTree_[node].minCoord[2], agents_[i]->position_.z());
}
if (end - begin > RVO_MAX_LEAF_SIZE) {
/* No leaf node. */
size_t coord;
if (agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] && agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
coord = 0;
} else if (agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
coord = 1;
} else {
coord = 2;
}
const float splitValue = 0.5f * (agentTree_[node].maxCoord[coord] + agentTree_[node].minCoord[coord]);
size_t left = begin;
size_t right = end;
while (left < right) {
while (left < right && agents_[left]->position_[coord] < splitValue) {
++left;
}
while (right > left && agents_[right - 1]->position_[coord] >= splitValue) {
--right;
}
if (left < right) {
std::swap(agents_[left], agents_[right - 1]);
++left;
--right;
}
}
size_t leftSize = left - begin;
if (leftSize == 0) {
++leftSize;
++left;
++right;
}
agentTree_[node].left = node + 1;
agentTree_[node].right = node + 2 * leftSize;
buildAgentTreeRecursive(begin, left, agentTree_[node].left);
buildAgentTreeRecursive(left, end, agentTree_[node].right);
}
}
void KdTree::computeAgentNeighbors(Agent *agent, float rangeSq) const {
queryAgentTreeRecursive(agent, rangeSq, 0);
}
void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const {
if (agentTree_[node].end - agentTree_[node].begin <= RVO_MAX_LEAF_SIZE) {
for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) {
agent->insertAgentNeighbor(agents_[i], rangeSq);
}
} else {
const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[0] - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxCoord[0])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[1] - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxCoord[1])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[2] - agent->position_.z())) + sqr(std::max(0.0f, agent->position_.z() - agentTree_[agentTree_[node].left].maxCoord[2]));
const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[0] - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxCoord[0])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[1] - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxCoord[1])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[2] - agent->position_.z())) + sqr(std::max(0.0f, agent->position_.z() - agentTree_[agentTree_[node].right].maxCoord[2]));
if (distSqLeft < distSqRight) {
if (distSqLeft < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
if (distSqRight < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
}
}
} else {
if (distSqRight < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
if (distSqLeft < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
}
}
}
}
}
} // namespace RVO

Some files were not shown because too many files have changed in this diff Show More