initial commit, 4.5 stable
Some checks failed
🔗 GHA / 📊 Static checks (push) Has been cancelled
🔗 GHA / 🤖 Android (push) Has been cancelled
🔗 GHA / 🍏 iOS (push) Has been cancelled
🔗 GHA / 🐧 Linux (push) Has been cancelled
🔗 GHA / 🍎 macOS (push) Has been cancelled
🔗 GHA / 🏁 Windows (push) Has been cancelled
🔗 GHA / 🌐 Web (push) Has been cancelled
Some checks failed
🔗 GHA / 📊 Static checks (push) Has been cancelled
🔗 GHA / 🤖 Android (push) Has been cancelled
🔗 GHA / 🍏 iOS (push) Has been cancelled
🔗 GHA / 🐧 Linux (push) Has been cancelled
🔗 GHA / 🍎 macOS (push) Has been cancelled
🔗 GHA / 🏁 Windows (push) Has been cancelled
🔗 GHA / 🌐 Web (push) Has been cancelled
This commit is contained in:
1553
modules/navigation_3d/3d/godot_navigation_server_3d.cpp
Normal file
1553
modules/navigation_3d/3d/godot_navigation_server_3d.cpp
Normal file
File diff suppressed because it is too large
Load Diff
305
modules/navigation_3d/3d/godot_navigation_server_3d.h
Normal file
305
modules/navigation_3d/3d/godot_navigation_server_3d.h
Normal file
@@ -0,0 +1,305 @@
|
||||
/**************************************************************************/
|
||||
/* godot_navigation_server_3d.h */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../nav_agent_3d.h"
|
||||
#include "../nav_link_3d.h"
|
||||
#include "../nav_map_3d.h"
|
||||
#include "../nav_obstacle_3d.h"
|
||||
#include "../nav_region_3d.h"
|
||||
|
||||
#include "core/templates/local_vector.h"
|
||||
#include "core/templates/rid.h"
|
||||
#include "core/templates/rid_owner.h"
|
||||
#include "servers/navigation/navigation_path_query_parameters_3d.h"
|
||||
#include "servers/navigation/navigation_path_query_result_3d.h"
|
||||
#include "servers/navigation_server_3d.h"
|
||||
|
||||
/// 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) override; \
|
||||
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) override; \
|
||||
void MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1)
|
||||
|
||||
class GodotNavigationServer3D;
|
||||
class NavMeshGenerator3D;
|
||||
|
||||
struct SetCommand3D {
|
||||
virtual ~SetCommand3D() {}
|
||||
virtual void exec(GodotNavigationServer3D *server) = 0;
|
||||
};
|
||||
|
||||
class GodotNavigationServer3D : public NavigationServer3D {
|
||||
Mutex commands_mutex;
|
||||
/// Mutex used to make any operation threadsafe.
|
||||
Mutex operations_mutex;
|
||||
|
||||
LocalVector<SetCommand3D *> commands;
|
||||
|
||||
mutable RID_Owner<NavLink3D> link_owner;
|
||||
mutable RID_Owner<NavMap3D> map_owner;
|
||||
mutable RID_Owner<NavRegion3D> region_owner;
|
||||
mutable RID_Owner<NavAgent3D> agent_owner;
|
||||
mutable RID_Owner<NavObstacle3D> obstacle_owner;
|
||||
|
||||
bool active = true;
|
||||
LocalVector<NavMap3D *> active_maps;
|
||||
|
||||
NavMeshGenerator3D *navmesh_generator_3d = nullptr;
|
||||
|
||||
// Performance Monitor
|
||||
int pm_region_count = 0;
|
||||
int pm_agent_count = 0;
|
||||
int pm_link_count = 0;
|
||||
int pm_polygon_count = 0;
|
||||
int pm_edge_count = 0;
|
||||
int pm_edge_merge_count = 0;
|
||||
int pm_edge_connection_count = 0;
|
||||
int pm_edge_free_count = 0;
|
||||
int pm_obstacle_count = 0;
|
||||
|
||||
public:
|
||||
GodotNavigationServer3D();
|
||||
virtual ~GodotNavigationServer3D();
|
||||
|
||||
void add_command(SetCommand3D *command);
|
||||
|
||||
virtual TypedArray<RID> get_maps() const override;
|
||||
|
||||
virtual RID map_create() override;
|
||||
COMMAND_2(map_set_active, RID, p_map, bool, p_active);
|
||||
virtual bool map_is_active(RID p_map) const override;
|
||||
|
||||
COMMAND_2(map_set_up, RID, p_map, Vector3, p_up);
|
||||
virtual Vector3 map_get_up(RID p_map) const override;
|
||||
|
||||
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 override;
|
||||
|
||||
COMMAND_2(map_set_cell_height, RID, p_map, real_t, p_cell_height);
|
||||
virtual real_t map_get_cell_height(RID p_map) const override;
|
||||
|
||||
COMMAND_2(map_set_merge_rasterizer_cell_scale, RID, p_map, float, p_value);
|
||||
virtual float map_get_merge_rasterizer_cell_scale(RID p_map) const override;
|
||||
|
||||
COMMAND_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled);
|
||||
virtual bool map_get_use_edge_connections(RID p_map) const override;
|
||||
|
||||
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 override;
|
||||
|
||||
COMMAND_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius);
|
||||
virtual real_t map_get_link_connection_radius(RID p_map) const override;
|
||||
|
||||
virtual Vector<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override;
|
||||
|
||||
virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const override;
|
||||
virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const override;
|
||||
virtual Vector3 map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const override;
|
||||
virtual RID map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const override;
|
||||
|
||||
virtual TypedArray<RID> map_get_links(RID p_map) const override;
|
||||
virtual TypedArray<RID> map_get_regions(RID p_map) const override;
|
||||
virtual TypedArray<RID> map_get_agents(RID p_map) const override;
|
||||
virtual TypedArray<RID> map_get_obstacles(RID p_map) const override;
|
||||
|
||||
virtual void map_force_update(RID p_map) override;
|
||||
virtual uint32_t map_get_iteration_id(RID p_map) const override;
|
||||
|
||||
COMMAND_2(map_set_use_async_iterations, RID, p_map, bool, p_enabled);
|
||||
virtual bool map_get_use_async_iterations(RID p_map) const override;
|
||||
|
||||
virtual Vector3 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const override;
|
||||
|
||||
virtual RID region_create() override;
|
||||
virtual uint32_t region_get_iteration_id(RID p_region) const override;
|
||||
|
||||
COMMAND_2(region_set_use_async_iterations, RID, p_region, bool, p_enabled);
|
||||
virtual bool region_get_use_async_iterations(RID p_region) const override;
|
||||
|
||||
COMMAND_2(region_set_enabled, RID, p_region, bool, p_enabled);
|
||||
virtual bool region_get_enabled(RID p_region) const override;
|
||||
|
||||
COMMAND_2(region_set_use_edge_connections, RID, p_region, bool, p_enabled);
|
||||
virtual bool region_get_use_edge_connections(RID p_region) const override;
|
||||
|
||||
COMMAND_2(region_set_enter_cost, RID, p_region, real_t, p_enter_cost);
|
||||
virtual real_t region_get_enter_cost(RID p_region) const override;
|
||||
COMMAND_2(region_set_travel_cost, RID, p_region, real_t, p_travel_cost);
|
||||
virtual real_t region_get_travel_cost(RID p_region) const override;
|
||||
|
||||
COMMAND_2(region_set_owner_id, RID, p_region, ObjectID, p_owner_id);
|
||||
virtual ObjectID region_get_owner_id(RID p_region) const override;
|
||||
|
||||
virtual bool region_owns_point(RID p_region, const Vector3 &p_point) const override;
|
||||
|
||||
COMMAND_2(region_set_map, RID, p_region, RID, p_map);
|
||||
virtual RID region_get_map(RID p_region) const override;
|
||||
COMMAND_2(region_set_navigation_layers, RID, p_region, uint32_t, p_navigation_layers);
|
||||
virtual uint32_t region_get_navigation_layers(RID p_region) const override;
|
||||
COMMAND_2(region_set_transform, RID, p_region, Transform3D, p_transform);
|
||||
virtual Transform3D region_get_transform(RID p_region) const override;
|
||||
COMMAND_2(region_set_navigation_mesh, RID, p_region, Ref<NavigationMesh>, p_navigation_mesh);
|
||||
#ifndef DISABLE_DEPRECATED
|
||||
virtual void region_bake_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh, Node *p_root_node) override;
|
||||
#endif // DISABLE_DEPRECATED
|
||||
virtual int region_get_connections_count(RID p_region) const override;
|
||||
virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override;
|
||||
virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override;
|
||||
virtual Vector3 region_get_closest_point_to_segment(RID p_region, const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision = false) const override;
|
||||
virtual Vector3 region_get_closest_point(RID p_region, const Vector3 &p_point) const override;
|
||||
virtual Vector3 region_get_closest_point_normal(RID p_region, const Vector3 &p_point) const override;
|
||||
virtual Vector3 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const override;
|
||||
virtual AABB region_get_bounds(RID p_region) const override;
|
||||
|
||||
virtual RID link_create() override;
|
||||
virtual uint32_t link_get_iteration_id(RID p_link) const override;
|
||||
COMMAND_2(link_set_map, RID, p_link, RID, p_map);
|
||||
virtual RID link_get_map(RID p_link) const override;
|
||||
COMMAND_2(link_set_enabled, RID, p_link, bool, p_enabled);
|
||||
virtual bool link_get_enabled(RID p_link) const override;
|
||||
COMMAND_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional);
|
||||
virtual bool link_is_bidirectional(RID p_link) const override;
|
||||
COMMAND_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers);
|
||||
virtual uint32_t link_get_navigation_layers(RID p_link) const override;
|
||||
COMMAND_2(link_set_start_position, RID, p_link, Vector3, p_position);
|
||||
virtual Vector3 link_get_start_position(RID p_link) const override;
|
||||
COMMAND_2(link_set_end_position, RID, p_link, Vector3, p_position);
|
||||
virtual Vector3 link_get_end_position(RID p_link) const override;
|
||||
COMMAND_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost);
|
||||
virtual real_t link_get_enter_cost(RID p_link) const override;
|
||||
COMMAND_2(link_set_travel_cost, RID, p_link, real_t, p_travel_cost);
|
||||
virtual real_t link_get_travel_cost(RID p_link) const override;
|
||||
COMMAND_2(link_set_owner_id, RID, p_link, ObjectID, p_owner_id);
|
||||
virtual ObjectID link_get_owner_id(RID p_link) const override;
|
||||
|
||||
virtual RID agent_create() override;
|
||||
COMMAND_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled);
|
||||
virtual bool agent_get_avoidance_enabled(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_use_3d_avoidance, RID, p_agent, bool, p_enabled);
|
||||
virtual bool agent_get_use_3d_avoidance(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_map, RID, p_agent, RID, p_map);
|
||||
virtual RID agent_get_map(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_paused, RID, p_agent, bool, p_paused);
|
||||
virtual bool agent_get_paused(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_distance);
|
||||
virtual real_t agent_get_neighbor_distance(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count);
|
||||
virtual int agent_get_max_neighbors(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon);
|
||||
virtual real_t agent_get_time_horizon_agents(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon);
|
||||
virtual real_t agent_get_time_horizon_obstacles(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius);
|
||||
virtual real_t agent_get_radius(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_height, RID, p_agent, real_t, p_height);
|
||||
virtual real_t agent_get_height(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed);
|
||||
virtual real_t agent_get_max_speed(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity);
|
||||
virtual Vector3 agent_get_velocity(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_velocity_forced, RID, p_agent, Vector3, p_velocity);
|
||||
COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position);
|
||||
virtual Vector3 agent_get_position(RID p_agent) const override;
|
||||
virtual bool agent_is_map_changed(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback);
|
||||
virtual bool agent_has_avoidance_callback(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers);
|
||||
virtual uint32_t agent_get_avoidance_layers(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask);
|
||||
virtual uint32_t agent_get_avoidance_mask(RID p_agent) const override;
|
||||
COMMAND_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority);
|
||||
virtual real_t agent_get_avoidance_priority(RID p_agent) const override;
|
||||
|
||||
virtual RID obstacle_create() override;
|
||||
COMMAND_2(obstacle_set_avoidance_enabled, RID, p_obstacle, bool, p_enabled);
|
||||
virtual bool obstacle_get_avoidance_enabled(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_use_3d_avoidance, RID, p_obstacle, bool, p_enabled);
|
||||
virtual bool obstacle_get_use_3d_avoidance(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_map, RID, p_obstacle, RID, p_map);
|
||||
virtual RID obstacle_get_map(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused);
|
||||
virtual bool obstacle_get_paused(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius);
|
||||
virtual real_t obstacle_get_radius(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_velocity, RID, p_obstacle, Vector3, p_velocity);
|
||||
virtual Vector3 obstacle_get_velocity(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector3, p_position);
|
||||
virtual Vector3 obstacle_get_position(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_height, RID, p_obstacle, real_t, p_height);
|
||||
virtual real_t obstacle_get_height(RID p_obstacle) const override;
|
||||
virtual void obstacle_set_vertices(RID p_obstacle, const Vector<Vector3> &p_vertices) override;
|
||||
virtual Vector<Vector3> obstacle_get_vertices(RID p_obstacle) const override;
|
||||
COMMAND_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers);
|
||||
virtual uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const override;
|
||||
|
||||
virtual void parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()) override;
|
||||
virtual void bake_from_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
|
||||
virtual void bake_from_source_geometry_data_async(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
|
||||
virtual bool is_baking_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) const override;
|
||||
virtual String get_baking_navigation_mesh_state_msg(Ref<NavigationMesh> p_navigation_mesh) const override;
|
||||
|
||||
virtual RID source_geometry_parser_create() override;
|
||||
virtual void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) override;
|
||||
|
||||
virtual Vector<Vector3> simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) override;
|
||||
|
||||
public:
|
||||
COMMAND_1(free, RID, p_object);
|
||||
|
||||
virtual void set_active(bool p_active) override;
|
||||
|
||||
void flush_queries();
|
||||
|
||||
virtual void process(double p_delta_time) override;
|
||||
virtual void physics_process(double p_delta_time) override;
|
||||
virtual void init() override;
|
||||
virtual void sync() override;
|
||||
virtual void finish() override;
|
||||
|
||||
virtual void query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback = Callable()) override;
|
||||
|
||||
int get_process_info(ProcessInfo p_info) const override;
|
||||
|
||||
private:
|
||||
void internal_free_agent(RID p_object);
|
||||
void internal_free_obstacle(RID p_object);
|
||||
};
|
||||
|
||||
#undef COMMAND_1
|
||||
#undef COMMAND_2
|
68
modules/navigation_3d/3d/nav_base_iteration_3d.h
Normal file
68
modules/navigation_3d/3d/nav_base_iteration_3d.h
Normal file
@@ -0,0 +1,68 @@
|
||||
/**************************************************************************/
|
||||
/* nav_base_iteration_3d.h */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../nav_utils_3d.h"
|
||||
|
||||
#include "core/object/ref_counted.h"
|
||||
#include "servers/navigation/navigation_utilities.h"
|
||||
|
||||
class NavBaseIteration3D : public RefCounted {
|
||||
GDCLASS(NavBaseIteration3D, RefCounted);
|
||||
|
||||
public:
|
||||
bool enabled = true;
|
||||
uint32_t navigation_layers = 1;
|
||||
real_t enter_cost = 0.0;
|
||||
real_t travel_cost = 1.0;
|
||||
NavigationUtilities::PathSegmentType owner_type;
|
||||
ObjectID owner_object_id;
|
||||
RID owner_rid;
|
||||
bool owner_use_edge_connections = false;
|
||||
LocalVector<Nav3D::Polygon> navmesh_polygons;
|
||||
LocalVector<LocalVector<Nav3D::Connection>> internal_connections;
|
||||
|
||||
bool get_enabled() const { return enabled; }
|
||||
NavigationUtilities::PathSegmentType get_type() const { return owner_type; }
|
||||
RID get_self() const { return owner_rid; }
|
||||
ObjectID get_owner_id() const { return owner_object_id; }
|
||||
uint32_t get_navigation_layers() const { return navigation_layers; }
|
||||
real_t get_enter_cost() const { return enter_cost; }
|
||||
real_t get_travel_cost() const { return travel_cost; }
|
||||
bool get_use_edge_connections() const { return owner_use_edge_connections; }
|
||||
const LocalVector<Nav3D::Polygon> &get_navmesh_polygons() const { return navmesh_polygons; }
|
||||
const LocalVector<LocalVector<Nav3D::Connection>> &get_internal_connections() const { return internal_connections; }
|
||||
|
||||
virtual ~NavBaseIteration3D() {
|
||||
navmesh_polygons.clear();
|
||||
internal_connections.clear();
|
||||
}
|
||||
};
|
426
modules/navigation_3d/3d/nav_map_builder_3d.cpp
Normal file
426
modules/navigation_3d/3d/nav_map_builder_3d.cpp
Normal file
@@ -0,0 +1,426 @@
|
||||
/**************************************************************************/
|
||||
/* nav_map_builder_3d.cpp */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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_builder_3d.h"
|
||||
|
||||
#include "../nav_link_3d.h"
|
||||
#include "../nav_map_3d.h"
|
||||
#include "../nav_region_3d.h"
|
||||
#include "nav_map_iteration_3d.h"
|
||||
#include "nav_region_iteration_3d.h"
|
||||
|
||||
using namespace Nav3D;
|
||||
|
||||
PointKey NavMapBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size) {
|
||||
const int x = static_cast<int>(Math::floor(p_pos.x / p_cell_size.x));
|
||||
const int y = static_cast<int>(Math::floor(p_pos.y / p_cell_size.y));
|
||||
const int z = static_cast<int>(Math::floor(p_pos.z / p_cell_size.z));
|
||||
|
||||
PointKey p;
|
||||
p.key = 0;
|
||||
p.x = x;
|
||||
p.y = y;
|
||||
p.z = z;
|
||||
return p;
|
||||
}
|
||||
|
||||
void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild3D &r_build) {
|
||||
PerformanceData &performance_data = r_build.performance_data;
|
||||
|
||||
performance_data.pm_polygon_count = 0;
|
||||
performance_data.pm_edge_count = 0;
|
||||
performance_data.pm_edge_merge_count = 0;
|
||||
performance_data.pm_edge_connection_count = 0;
|
||||
performance_data.pm_edge_free_count = 0;
|
||||
|
||||
_build_step_gather_region_polygons(r_build);
|
||||
|
||||
_build_step_find_edge_connection_pairs(r_build);
|
||||
|
||||
_build_step_merge_edge_connection_pairs(r_build);
|
||||
|
||||
_build_step_edge_connection_margin_connections(r_build);
|
||||
|
||||
_build_step_navlink_connections(r_build);
|
||||
|
||||
_build_update_map_iteration(r_build);
|
||||
}
|
||||
|
||||
void NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild3D &r_build) {
|
||||
PerformanceData &performance_data = r_build.performance_data;
|
||||
NavMapIteration3D *map_iteration = r_build.map_iteration;
|
||||
|
||||
const LocalVector<Ref<NavRegionIteration3D>> ®ions = map_iteration->region_iterations;
|
||||
HashMap<const NavBaseIteration3D *, LocalVector<Connection>> ®ion_external_connections = map_iteration->external_region_connections;
|
||||
|
||||
map_iteration->navbases_polygons_external_connections.clear();
|
||||
|
||||
// Remove regions connections.
|
||||
region_external_connections.clear();
|
||||
|
||||
// Copy all region polygons in the map.
|
||||
int polygon_count = 0;
|
||||
for (const Ref<NavRegionIteration3D> ®ion : regions) {
|
||||
const uint32_t polygons_size = region->navmesh_polygons.size();
|
||||
polygon_count += polygons_size;
|
||||
|
||||
region_external_connections[region.ptr()] = LocalVector<Connection>();
|
||||
map_iteration->navbases_polygons_external_connections[region.ptr()] = LocalVector<LocalVector<Connection>>();
|
||||
map_iteration->navbases_polygons_external_connections[region.ptr()].resize(polygons_size);
|
||||
}
|
||||
|
||||
performance_data.pm_polygon_count = polygon_count;
|
||||
r_build.polygon_count = polygon_count;
|
||||
}
|
||||
|
||||
void NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuild3D &r_build) {
|
||||
PerformanceData &performance_data = r_build.performance_data;
|
||||
NavMapIteration3D *map_iteration = r_build.map_iteration;
|
||||
int polygon_count = r_build.polygon_count;
|
||||
|
||||
HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
|
||||
|
||||
// Group all edges per key.
|
||||
connection_pairs_map.clear();
|
||||
connection_pairs_map.reserve(polygon_count);
|
||||
int free_edges_count = 0; // How many ConnectionPairs have only one Connection.
|
||||
|
||||
for (const Ref<NavRegionIteration3D> ®ion : map_iteration->region_iterations) {
|
||||
for (const ConnectableEdge &connectable_edge : region->get_external_edges()) {
|
||||
const EdgeKey &ek = connectable_edge.ek;
|
||||
|
||||
HashMap<EdgeKey, EdgeConnectionPair, EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
|
||||
if (!pair_it) {
|
||||
pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair());
|
||||
performance_data.pm_edge_count += 1;
|
||||
++free_edges_count;
|
||||
}
|
||||
EdgeConnectionPair &pair = pair_it->value;
|
||||
if (pair.size < 2) {
|
||||
// Add the polygon/edge tuple to this key.
|
||||
Connection new_connection;
|
||||
new_connection.polygon = ®ion->navmesh_polygons[connectable_edge.polygon_index];
|
||||
new_connection.edge = connectable_edge.edge;
|
||||
new_connection.pathway_start = connectable_edge.pathway_start;
|
||||
new_connection.pathway_end = connectable_edge.pathway_end;
|
||||
|
||||
pair.connections[pair.size] = new_connection;
|
||||
++pair.size;
|
||||
if (pair.size == 2) {
|
||||
--free_edges_count;
|
||||
}
|
||||
|
||||
} else {
|
||||
// The edge is already connected with another edge, skip.
|
||||
ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
r_build.free_edge_count = free_edges_count;
|
||||
}
|
||||
|
||||
void NavMapBuilder3D::_build_step_merge_edge_connection_pairs(NavMapIterationBuild3D &r_build) {
|
||||
PerformanceData &performance_data = r_build.performance_data;
|
||||
|
||||
HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
|
||||
LocalVector<Connection> &free_edges = r_build.iter_free_edges;
|
||||
int free_edges_count = r_build.free_edge_count;
|
||||
bool use_edge_connections = r_build.use_edge_connections;
|
||||
|
||||
free_edges.clear();
|
||||
free_edges.reserve(free_edges_count);
|
||||
|
||||
NavMapIteration3D *map_iteration = r_build.map_iteration;
|
||||
|
||||
HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::Connection>>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections;
|
||||
|
||||
for (const KeyValue<EdgeKey, EdgeConnectionPair> &pair_it : connection_pairs_map) {
|
||||
const EdgeConnectionPair &pair = pair_it.value;
|
||||
if (pair.size == 2) {
|
||||
// Connect edge that are shared in different polygons.
|
||||
const Connection &c1 = pair.connections[0];
|
||||
const Connection &c2 = pair.connections[1];
|
||||
|
||||
navbases_polygons_external_connections[c1.polygon->owner][c1.polygon->id].push_back(c2);
|
||||
navbases_polygons_external_connections[c2.polygon->owner][c2.polygon->id].push_back(c1);
|
||||
performance_data.pm_edge_connection_count += 1;
|
||||
|
||||
} else {
|
||||
CRASH_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size));
|
||||
if (use_edge_connections && pair.connections[0].polygon->owner->get_use_edge_connections()) {
|
||||
free_edges.push_back(pair.connections[0]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapIterationBuild3D &r_build) {
|
||||
PerformanceData &performance_data = r_build.performance_data;
|
||||
NavMapIteration3D *map_iteration = r_build.map_iteration;
|
||||
|
||||
real_t edge_connection_margin = r_build.edge_connection_margin;
|
||||
|
||||
LocalVector<Connection> &free_edges = r_build.iter_free_edges;
|
||||
HashMap<const NavBaseIteration3D *, LocalVector<Connection>> ®ion_external_connections = map_iteration->external_region_connections;
|
||||
|
||||
HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::Connection>>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections;
|
||||
|
||||
// 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.
|
||||
performance_data.pm_edge_free_count = free_edges.size();
|
||||
|
||||
const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin;
|
||||
|
||||
for (uint32_t i = 0; i < free_edges.size(); i++) {
|
||||
const Connection &free_edge = free_edges[i];
|
||||
const Vector3 &edge_p1 = free_edge.pathway_start;
|
||||
const Vector3 &edge_p2 = free_edge.pathway_end;
|
||||
|
||||
for (uint32_t j = 0; j < free_edges.size(); j++) {
|
||||
const Connection &other_edge = free_edges[j];
|
||||
if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const Vector3 &other_edge_p1 = other_edge.pathway_start;
|
||||
const Vector3 &other_edge_p2 = other_edge.pathway_end;
|
||||
|
||||
// Compute the projection of the opposite edge on the current one
|
||||
Vector3 edge_vector = edge_p2 - edge_p1;
|
||||
real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector.length_squared());
|
||||
real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector.length_squared());
|
||||
if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check if the two edges are close to each other enough and compute a pathway between the two regions.
|
||||
Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
|
||||
Vector3 other1;
|
||||
if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) {
|
||||
other1 = other_edge_p1;
|
||||
} else {
|
||||
other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
|
||||
}
|
||||
if (other1.distance_squared_to(self1) > edge_connection_margin_squared) {
|
||||
continue;
|
||||
}
|
||||
|
||||
Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
|
||||
Vector3 other2;
|
||||
if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) {
|
||||
other2 = other_edge_p2;
|
||||
} else {
|
||||
other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
|
||||
}
|
||||
if (other2.distance_squared_to(self2) > edge_connection_margin_squared) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// The edges can now be connected.
|
||||
Connection new_connection = other_edge;
|
||||
new_connection.pathway_start = (self1 + other1) / 2.0;
|
||||
new_connection.pathway_end = (self2 + other2) / 2.0;
|
||||
//free_edge.polygon->connections.push_back(new_connection);
|
||||
|
||||
// Add the connection to the region_connection map.
|
||||
region_external_connections[free_edge.polygon->owner].push_back(new_connection);
|
||||
navbases_polygons_external_connections[free_edge.polygon->owner][free_edge.polygon->id].push_back(new_connection);
|
||||
performance_data.pm_edge_connection_count += 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild3D &r_build) {
|
||||
NavMapIteration3D *map_iteration = r_build.map_iteration;
|
||||
|
||||
real_t link_connection_radius = r_build.link_connection_radius;
|
||||
|
||||
const LocalVector<Ref<NavLinkIteration3D>> &links = map_iteration->link_iterations;
|
||||
|
||||
int polygon_count = r_build.polygon_count;
|
||||
|
||||
real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius;
|
||||
|
||||
HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::Connection>>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections;
|
||||
LocalVector<Nav3D::Polygon> &navlink_polygons = map_iteration->navlink_polygons;
|
||||
navlink_polygons.clear();
|
||||
navlink_polygons.resize(links.size());
|
||||
uint32_t navlink_index = 0;
|
||||
|
||||
// Search for polygons within range of a nav link.
|
||||
for (const Ref<NavLinkIteration3D> &link : links) {
|
||||
polygon_count++;
|
||||
Polygon &new_polygon = navlink_polygons[navlink_index++];
|
||||
|
||||
new_polygon.id = 0;
|
||||
new_polygon.owner = link.ptr();
|
||||
|
||||
const Vector3 link_start_pos = link->get_start_position();
|
||||
const Vector3 link_end_pos = link->get_end_position();
|
||||
|
||||
Polygon *closest_start_polygon = nullptr;
|
||||
real_t closest_start_sqr_dist = link_connection_radius_sqr;
|
||||
Vector3 closest_start_point;
|
||||
|
||||
Polygon *closest_end_polygon = nullptr;
|
||||
real_t closest_end_sqr_dist = link_connection_radius_sqr;
|
||||
Vector3 closest_end_point;
|
||||
|
||||
for (const Ref<NavRegionIteration3D> ®ion : map_iteration->region_iterations) {
|
||||
AABB region_bounds = region->get_bounds().grow(link_connection_radius);
|
||||
if (!region_bounds.has_point(link_start_pos) && !region_bounds.has_point(link_end_pos)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
for (Polygon &polyon : region->navmesh_polygons) {
|
||||
for (uint32_t point_id = 2; point_id < polyon.vertices.size(); point_id += 1) {
|
||||
const Face3 face(polyon.vertices[0], polyon.vertices[point_id - 1], polyon.vertices[point_id]);
|
||||
|
||||
{
|
||||
const Vector3 start_point = face.get_closest_point_to(link_start_pos);
|
||||
const real_t sqr_dist = start_point.distance_squared_to(link_start_pos);
|
||||
|
||||
// Pick the polygon that is within our radius and is closer than anything we've seen yet.
|
||||
if (sqr_dist < closest_start_sqr_dist) {
|
||||
closest_start_sqr_dist = sqr_dist;
|
||||
closest_start_point = start_point;
|
||||
closest_start_polygon = &polyon;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
const Vector3 end_point = face.get_closest_point_to(link_end_pos);
|
||||
const real_t sqr_dist = end_point.distance_squared_to(link_end_pos);
|
||||
|
||||
// Pick the polygon that is within our radius and is closer than anything we've seen yet.
|
||||
if (sqr_dist < closest_end_sqr_dist) {
|
||||
closest_end_sqr_dist = sqr_dist;
|
||||
closest_end_point = end_point;
|
||||
closest_end_polygon = &polyon;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// If we have both a start and end point, then create a synthetic polygon to route through.
|
||||
if (closest_start_polygon && closest_end_polygon) {
|
||||
new_polygon.vertices.resize(4);
|
||||
|
||||
// Build a set of vertices that create a thin polygon going from the start to the end point.
|
||||
new_polygon.vertices[0] = closest_start_point;
|
||||
new_polygon.vertices[1] = closest_start_point;
|
||||
new_polygon.vertices[2] = closest_end_point;
|
||||
new_polygon.vertices[3] = closest_end_point;
|
||||
|
||||
// Setup connections to go forward in the link.
|
||||
{
|
||||
Connection entry_connection;
|
||||
entry_connection.polygon = &new_polygon;
|
||||
entry_connection.edge = -1;
|
||||
entry_connection.pathway_start = new_polygon.vertices[0];
|
||||
entry_connection.pathway_end = new_polygon.vertices[1];
|
||||
navbases_polygons_external_connections[closest_start_polygon->owner][closest_start_polygon->id].push_back(entry_connection);
|
||||
|
||||
Connection exit_connection;
|
||||
exit_connection.polygon = closest_end_polygon;
|
||||
exit_connection.edge = -1;
|
||||
exit_connection.pathway_start = new_polygon.vertices[2];
|
||||
exit_connection.pathway_end = new_polygon.vertices[3];
|
||||
navbases_polygons_external_connections[link.ptr()].push_back(LocalVector<Nav3D::Connection>());
|
||||
navbases_polygons_external_connections[link.ptr()][new_polygon.id].push_back(exit_connection);
|
||||
}
|
||||
|
||||
// If the link is bi-directional, create connections from the end to the start.
|
||||
if (link->is_bidirectional()) {
|
||||
Connection entry_connection;
|
||||
entry_connection.polygon = &new_polygon;
|
||||
entry_connection.edge = -1;
|
||||
entry_connection.pathway_start = new_polygon.vertices[2];
|
||||
entry_connection.pathway_end = new_polygon.vertices[3];
|
||||
navbases_polygons_external_connections[closest_end_polygon->owner][closest_end_polygon->id].push_back(entry_connection);
|
||||
|
||||
Connection exit_connection;
|
||||
exit_connection.polygon = closest_start_polygon;
|
||||
exit_connection.edge = -1;
|
||||
exit_connection.pathway_start = new_polygon.vertices[0];
|
||||
exit_connection.pathway_end = new_polygon.vertices[1];
|
||||
navbases_polygons_external_connections[link.ptr()].push_back(LocalVector<Nav3D::Connection>());
|
||||
navbases_polygons_external_connections[link.ptr()][new_polygon.id].push_back(exit_connection);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
r_build.polygon_count = polygon_count;
|
||||
}
|
||||
|
||||
void NavMapBuilder3D::_build_update_map_iteration(NavMapIterationBuild3D &r_build) {
|
||||
NavMapIteration3D *map_iteration = r_build.map_iteration;
|
||||
|
||||
map_iteration->navmesh_polygon_count = r_build.polygon_count;
|
||||
|
||||
uint32_t navmesh_polygon_count = r_build.polygon_count;
|
||||
uint32_t total_polygon_count = navmesh_polygon_count;
|
||||
|
||||
map_iteration->path_query_slots_mutex.lock();
|
||||
for (NavMeshQueries3D::PathQuerySlot &p_path_query_slot : map_iteration->path_query_slots) {
|
||||
p_path_query_slot.traversable_polys.clear();
|
||||
p_path_query_slot.traversable_polys.reserve(navmesh_polygon_count * 0.25);
|
||||
p_path_query_slot.path_corridor.clear();
|
||||
|
||||
p_path_query_slot.path_corridor.resize(total_polygon_count);
|
||||
|
||||
p_path_query_slot.poly_to_id.clear();
|
||||
p_path_query_slot.poly_to_id.reserve(total_polygon_count);
|
||||
|
||||
int polygon_id = 0;
|
||||
for (Ref<NavRegionIteration3D> ®ion : map_iteration->region_iterations) {
|
||||
for (const Polygon &polygon : region->navmesh_polygons) {
|
||||
p_path_query_slot.poly_to_id[&polygon] = polygon_id;
|
||||
polygon_id++;
|
||||
}
|
||||
}
|
||||
|
||||
for (const Polygon &polygon : map_iteration->navlink_polygons) {
|
||||
p_path_query_slot.poly_to_id[&polygon] = polygon_id;
|
||||
polygon_id++;
|
||||
}
|
||||
|
||||
DEV_ASSERT(p_path_query_slot.path_corridor.size() == p_path_query_slot.poly_to_id.size());
|
||||
}
|
||||
|
||||
map_iteration->path_query_slots_mutex.unlock();
|
||||
}
|
49
modules/navigation_3d/3d/nav_map_builder_3d.h
Normal file
49
modules/navigation_3d/3d/nav_map_builder_3d.h
Normal file
@@ -0,0 +1,49 @@
|
||||
/**************************************************************************/
|
||||
/* nav_map_builder_3d.h */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../nav_utils_3d.h"
|
||||
|
||||
struct NavMapIterationBuild3D;
|
||||
|
||||
class NavMapBuilder3D {
|
||||
static void _build_step_gather_region_polygons(NavMapIterationBuild3D &r_build);
|
||||
static void _build_step_find_edge_connection_pairs(NavMapIterationBuild3D &r_build);
|
||||
static void _build_step_merge_edge_connection_pairs(NavMapIterationBuild3D &r_build);
|
||||
static void _build_step_edge_connection_margin_connections(NavMapIterationBuild3D &r_build);
|
||||
static void _build_step_navlink_connections(NavMapIterationBuild3D &r_build);
|
||||
static void _build_update_map_iteration(NavMapIterationBuild3D &r_build);
|
||||
|
||||
public:
|
||||
static Nav3D::PointKey get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size);
|
||||
|
||||
static void build_navmap_iteration(NavMapIterationBuild3D &r_build);
|
||||
};
|
122
modules/navigation_3d/3d/nav_map_iteration_3d.h
Normal file
122
modules/navigation_3d/3d/nav_map_iteration_3d.h
Normal file
@@ -0,0 +1,122 @@
|
||||
/**************************************************************************/
|
||||
/* nav_map_iteration_3d.h */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../nav_rid_3d.h"
|
||||
#include "../nav_utils_3d.h"
|
||||
#include "nav_mesh_queries_3d.h"
|
||||
|
||||
#include "core/math/math_defs.h"
|
||||
#include "core/os/semaphore.h"
|
||||
|
||||
class NavLinkIteration3D;
|
||||
class NavRegion3D;
|
||||
class NavRegionIteration3D;
|
||||
struct NavMapIteration3D;
|
||||
|
||||
struct NavMapIterationBuild3D {
|
||||
Vector3 merge_rasterizer_cell_size;
|
||||
bool use_edge_connections = true;
|
||||
real_t edge_connection_margin;
|
||||
real_t link_connection_radius;
|
||||
Nav3D::PerformanceData performance_data;
|
||||
int polygon_count = 0;
|
||||
int free_edge_count = 0;
|
||||
|
||||
HashMap<Nav3D::EdgeKey, Nav3D::EdgeConnectionPair, Nav3D::EdgeKey> iter_connection_pairs_map;
|
||||
LocalVector<Nav3D::Connection> iter_free_edges;
|
||||
|
||||
NavMapIteration3D *map_iteration = nullptr;
|
||||
|
||||
int navmesh_polygon_count = 0;
|
||||
|
||||
void reset() {
|
||||
performance_data.reset();
|
||||
|
||||
iter_connection_pairs_map.clear();
|
||||
iter_free_edges.clear();
|
||||
polygon_count = 0;
|
||||
free_edge_count = 0;
|
||||
|
||||
navmesh_polygon_count = 0;
|
||||
}
|
||||
};
|
||||
|
||||
struct NavMapIteration3D {
|
||||
mutable SafeNumeric<uint32_t> users;
|
||||
RWLock rwlock;
|
||||
|
||||
Vector3 map_up;
|
||||
|
||||
LocalVector<Ref<NavRegionIteration3D>> region_iterations;
|
||||
LocalVector<Ref<NavLinkIteration3D>> link_iterations;
|
||||
|
||||
int navmesh_polygon_count = 0;
|
||||
|
||||
// The edge connections that the map builds on top with the edge connection margin.
|
||||
HashMap<const NavBaseIteration3D *, LocalVector<Nav3D::Connection>> external_region_connections;
|
||||
HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::Connection>>> navbases_polygons_external_connections;
|
||||
|
||||
LocalVector<Nav3D::Polygon> navlink_polygons;
|
||||
|
||||
HashMap<NavRegion3D *, Ref<NavRegionIteration3D>> region_ptr_to_region_iteration;
|
||||
|
||||
LocalVector<NavMeshQueries3D::PathQuerySlot> path_query_slots;
|
||||
Mutex path_query_slots_mutex;
|
||||
Semaphore path_query_slots_semaphore;
|
||||
|
||||
void clear() {
|
||||
map_up = Vector3();
|
||||
navmesh_polygon_count = 0;
|
||||
|
||||
region_iterations.clear();
|
||||
link_iterations.clear();
|
||||
external_region_connections.clear();
|
||||
navbases_polygons_external_connections.clear();
|
||||
navlink_polygons.clear();
|
||||
region_ptr_to_region_iteration.clear();
|
||||
}
|
||||
};
|
||||
|
||||
class NavMapIterationRead3D {
|
||||
const NavMapIteration3D &map_iteration;
|
||||
|
||||
public:
|
||||
_ALWAYS_INLINE_ NavMapIterationRead3D(const NavMapIteration3D &p_iteration) :
|
||||
map_iteration(p_iteration) {
|
||||
map_iteration.rwlock.read_lock();
|
||||
map_iteration.users.increment();
|
||||
}
|
||||
_ALWAYS_INLINE_ ~NavMapIterationRead3D() {
|
||||
map_iteration.users.decrement();
|
||||
map_iteration.rwlock.read_unlock();
|
||||
}
|
||||
};
|
597
modules/navigation_3d/3d/nav_mesh_generator_3d.cpp
Normal file
597
modules/navigation_3d/3d/nav_mesh_generator_3d.cpp
Normal file
@@ -0,0 +1,597 @@
|
||||
/**************************************************************************/
|
||||
/* nav_mesh_generator_3d.cpp */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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_mesh_generator_3d.h"
|
||||
|
||||
#include "core/config/project_settings.h"
|
||||
#include "core/os/thread.h"
|
||||
#include "scene/3d/node_3d.h"
|
||||
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
|
||||
#include "scene/resources/navigation_mesh.h"
|
||||
|
||||
#include <Recast.h>
|
||||
|
||||
NavMeshGenerator3D *NavMeshGenerator3D::singleton = nullptr;
|
||||
Mutex NavMeshGenerator3D::baking_navmesh_mutex;
|
||||
Mutex NavMeshGenerator3D::generator_task_mutex;
|
||||
RWLock NavMeshGenerator3D::generator_parsers_rwlock;
|
||||
bool NavMeshGenerator3D::use_threads = true;
|
||||
bool NavMeshGenerator3D::baking_use_multiple_threads = true;
|
||||
bool NavMeshGenerator3D::baking_use_high_priority_threads = true;
|
||||
HashMap<Ref<NavigationMesh>, NavMeshGenerator3D::NavMeshGeneratorTask3D *> NavMeshGenerator3D::baking_navmeshes;
|
||||
HashMap<WorkerThreadPool::TaskID, NavMeshGenerator3D::NavMeshGeneratorTask3D *> NavMeshGenerator3D::generator_tasks;
|
||||
LocalVector<NavMeshGeometryParser3D *> NavMeshGenerator3D::generator_parsers;
|
||||
|
||||
static const char *_navmesh_bake_state_msgs[(size_t)NavMeshGenerator3D::NavMeshBakeState::BAKE_STATE_MAX] = {
|
||||
"",
|
||||
"Setting up configuration...",
|
||||
"Calculating grid size...",
|
||||
"Creating heightfield...",
|
||||
"Marking walkable triangles...",
|
||||
"Constructing compact heightfield...", // step 5
|
||||
"Eroding walkable area...",
|
||||
"Sample partitioning...",
|
||||
"Creating contours...",
|
||||
"Creating polymesh...",
|
||||
"Converting to native navigation mesh...", // step 10
|
||||
"Baking cleanup...",
|
||||
"Baking finished.",
|
||||
};
|
||||
|
||||
NavMeshGenerator3D *NavMeshGenerator3D::get_singleton() {
|
||||
return singleton;
|
||||
}
|
||||
|
||||
NavMeshGenerator3D::NavMeshGenerator3D() {
|
||||
ERR_FAIL_COND(singleton != nullptr);
|
||||
singleton = this;
|
||||
|
||||
baking_use_multiple_threads = GLOBAL_GET("navigation/baking/thread_model/baking_use_multiple_threads");
|
||||
baking_use_high_priority_threads = GLOBAL_GET("navigation/baking/thread_model/baking_use_high_priority_threads");
|
||||
|
||||
// Using threads might cause problems on certain exports or with the Editor on certain devices.
|
||||
// This is the main switch to turn threaded navmesh baking off should the need arise.
|
||||
use_threads = baking_use_multiple_threads;
|
||||
}
|
||||
|
||||
NavMeshGenerator3D::~NavMeshGenerator3D() {
|
||||
cleanup();
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::sync() {
|
||||
if (generator_tasks.is_empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
|
||||
{
|
||||
MutexLock generator_task_lock(generator_task_mutex);
|
||||
|
||||
LocalVector<WorkerThreadPool::TaskID> finished_task_ids;
|
||||
|
||||
for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) {
|
||||
if (WorkerThreadPool::get_singleton()->is_task_completed(E.key)) {
|
||||
WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
|
||||
finished_task_ids.push_back(E.key);
|
||||
|
||||
NavMeshGeneratorTask3D *generator_task = E.value;
|
||||
DEV_ASSERT(generator_task->status == NavMeshGeneratorTask3D::TaskStatus::BAKING_FINISHED);
|
||||
|
||||
baking_navmeshes.erase(generator_task->navigation_mesh);
|
||||
if (generator_task->callback.is_valid()) {
|
||||
generator_emit_callback(generator_task->callback);
|
||||
}
|
||||
generator_task->navigation_mesh->emit_changed();
|
||||
memdelete(generator_task);
|
||||
}
|
||||
}
|
||||
|
||||
for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) {
|
||||
generator_tasks.erase(finished_task_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::cleanup() {
|
||||
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
|
||||
{
|
||||
MutexLock generator_task_lock(generator_task_mutex);
|
||||
|
||||
baking_navmeshes.clear();
|
||||
|
||||
for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) {
|
||||
WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
|
||||
NavMeshGeneratorTask3D *generator_task = E.value;
|
||||
memdelete(generator_task);
|
||||
}
|
||||
generator_tasks.clear();
|
||||
|
||||
generator_parsers_rwlock.write_lock();
|
||||
generator_parsers.clear();
|
||||
generator_parsers_rwlock.write_unlock();
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::finish() {
|
||||
cleanup();
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::parse_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
|
||||
ERR_FAIL_COND(!Thread::is_main_thread());
|
||||
ERR_FAIL_COND(p_navigation_mesh.is_null());
|
||||
ERR_FAIL_NULL(p_root_node);
|
||||
ERR_FAIL_COND(!p_root_node->is_inside_tree());
|
||||
ERR_FAIL_COND(p_source_geometry_data.is_null());
|
||||
|
||||
generator_parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node);
|
||||
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback) {
|
||||
ERR_FAIL_COND(p_navigation_mesh.is_null());
|
||||
ERR_FAIL_COND(p_source_geometry_data.is_null());
|
||||
|
||||
if (!p_source_geometry_data->has_data()) {
|
||||
p_navigation_mesh->clear();
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
p_navigation_mesh->emit_changed();
|
||||
return;
|
||||
}
|
||||
|
||||
if (is_baking(p_navigation_mesh)) {
|
||||
ERR_FAIL_MSG("NavigationMesh is already baking. Wait for current bake to finish.");
|
||||
}
|
||||
baking_navmesh_mutex.lock();
|
||||
NavMeshGeneratorTask3D generator_task;
|
||||
baking_navmeshes.insert(p_navigation_mesh, &generator_task);
|
||||
baking_navmesh_mutex.unlock();
|
||||
|
||||
generator_task.navigation_mesh = p_navigation_mesh;
|
||||
generator_task.source_geometry_data = p_source_geometry_data;
|
||||
generator_task.status = NavMeshGeneratorTask3D::TaskStatus::BAKING_STARTED;
|
||||
|
||||
generator_bake_from_source_geometry_data(&generator_task);
|
||||
|
||||
baking_navmesh_mutex.lock();
|
||||
baking_navmeshes.erase(p_navigation_mesh);
|
||||
baking_navmesh_mutex.unlock();
|
||||
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
|
||||
p_navigation_mesh->emit_changed();
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::bake_from_source_geometry_data_async(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback) {
|
||||
ERR_FAIL_COND(p_navigation_mesh.is_null());
|
||||
ERR_FAIL_COND(p_source_geometry_data.is_null());
|
||||
|
||||
if (!p_source_geometry_data->has_data()) {
|
||||
p_navigation_mesh->clear();
|
||||
if (p_callback.is_valid()) {
|
||||
generator_emit_callback(p_callback);
|
||||
}
|
||||
p_navigation_mesh->emit_changed();
|
||||
return;
|
||||
}
|
||||
|
||||
if (!use_threads) {
|
||||
bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
|
||||
return;
|
||||
}
|
||||
|
||||
if (is_baking(p_navigation_mesh)) {
|
||||
ERR_FAIL_MSG("NavigationMesh is already baking. Wait for current bake to finish.");
|
||||
return;
|
||||
}
|
||||
baking_navmesh_mutex.lock();
|
||||
NavMeshGeneratorTask3D *generator_task = memnew(NavMeshGeneratorTask3D);
|
||||
baking_navmeshes.insert(p_navigation_mesh, generator_task);
|
||||
baking_navmesh_mutex.unlock();
|
||||
|
||||
generator_task->navigation_mesh = p_navigation_mesh;
|
||||
generator_task->source_geometry_data = p_source_geometry_data;
|
||||
generator_task->callback = p_callback;
|
||||
generator_task->status = NavMeshGeneratorTask3D::TaskStatus::BAKING_STARTED;
|
||||
generator_task->thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMeshGenerator3D::generator_thread_bake, generator_task, NavMeshGenerator3D::baking_use_high_priority_threads, SNAME("NavMeshGeneratorBake3D"));
|
||||
MutexLock generator_task_lock(generator_task_mutex);
|
||||
generator_tasks.insert(generator_task->thread_task_id, generator_task);
|
||||
}
|
||||
|
||||
bool NavMeshGenerator3D::is_baking(Ref<NavigationMesh> p_navigation_mesh) {
|
||||
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
|
||||
return baking_navmeshes.has(p_navigation_mesh);
|
||||
}
|
||||
|
||||
String NavMeshGenerator3D::get_baking_state_msg(Ref<NavigationMesh> p_navigation_mesh) {
|
||||
String bake_state_msg;
|
||||
MutexLock baking_navmesh_lock(baking_navmesh_mutex);
|
||||
if (baking_navmeshes.has(p_navigation_mesh)) {
|
||||
bake_state_msg = _navmesh_bake_state_msgs[baking_navmeshes[p_navigation_mesh]->bake_state];
|
||||
} else {
|
||||
bake_state_msg = _navmesh_bake_state_msgs[NavMeshBakeState::BAKE_STATE_NONE];
|
||||
}
|
||||
return bake_state_msg;
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::generator_thread_bake(void *p_arg) {
|
||||
NavMeshGeneratorTask3D *generator_task = static_cast<NavMeshGeneratorTask3D *>(p_arg);
|
||||
|
||||
generator_bake_from_source_geometry_data(generator_task);
|
||||
|
||||
generator_task->status = NavMeshGeneratorTask3D::TaskStatus::BAKING_FINISHED;
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::generator_parse_geometry_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node, bool p_recurse_children) {
|
||||
generator_parsers_rwlock.read_lock();
|
||||
for (const NavMeshGeometryParser3D *parser : generator_parsers) {
|
||||
if (!parser->callback.is_valid()) {
|
||||
continue;
|
||||
}
|
||||
parser->callback.call(p_navigation_mesh, p_source_geometry_data, p_node);
|
||||
}
|
||||
generator_parsers_rwlock.read_unlock();
|
||||
|
||||
if (p_recurse_children) {
|
||||
for (int i = 0; i < p_node->get_child_count(); i++) {
|
||||
generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, p_node->get_child(i), p_recurse_children);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::set_generator_parsers(LocalVector<NavMeshGeometryParser3D *> p_parsers) {
|
||||
RWLockWrite write_lock(generator_parsers_rwlock);
|
||||
generator_parsers = p_parsers;
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::generator_parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node) {
|
||||
List<Node *> parse_nodes;
|
||||
|
||||
if (p_navigation_mesh->get_source_geometry_mode() == NavigationMesh::SOURCE_GEOMETRY_ROOT_NODE_CHILDREN) {
|
||||
parse_nodes.push_back(p_root_node);
|
||||
} else {
|
||||
p_root_node->get_tree()->get_nodes_in_group(p_navigation_mesh->get_source_group_name(), &parse_nodes);
|
||||
}
|
||||
|
||||
Transform3D root_node_transform = Transform3D();
|
||||
if (Object::cast_to<Node3D>(p_root_node)) {
|
||||
root_node_transform = Object::cast_to<Node3D>(p_root_node)->get_global_transform().affine_inverse();
|
||||
}
|
||||
|
||||
p_source_geometry_data->clear();
|
||||
p_source_geometry_data->root_node_transform = root_node_transform;
|
||||
|
||||
bool recurse_children = p_navigation_mesh->get_source_geometry_mode() != NavigationMesh::SOURCE_GEOMETRY_GROUPS_EXPLICIT;
|
||||
|
||||
for (Node *parse_node : parse_nodes) {
|
||||
generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, parse_node, recurse_children);
|
||||
}
|
||||
}
|
||||
|
||||
void NavMeshGenerator3D::generator_bake_from_source_geometry_data(NavMeshGeneratorTask3D *p_generator_task) {
|
||||
Ref<NavigationMesh> p_navigation_mesh = p_generator_task->navigation_mesh;
|
||||
const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data = p_generator_task->source_geometry_data;
|
||||
|
||||
if (p_navigation_mesh.is_null() || p_source_geometry_data.is_null()) {
|
||||
return;
|
||||
}
|
||||
|
||||
Vector<float> source_geometry_vertices;
|
||||
Vector<int> source_geometry_indices;
|
||||
Vector<NavigationMeshSourceGeometryData3D::ProjectedObstruction> projected_obstructions;
|
||||
|
||||
p_source_geometry_data->get_data(
|
||||
source_geometry_vertices,
|
||||
source_geometry_indices,
|
||||
projected_obstructions);
|
||||
|
||||
if (source_geometry_vertices.size() < 3 || source_geometry_indices.size() < 3) {
|
||||
return;
|
||||
}
|
||||
|
||||
rcHeightfield *hf = nullptr;
|
||||
rcCompactHeightfield *chf = nullptr;
|
||||
rcContourSet *cset = nullptr;
|
||||
rcPolyMesh *poly_mesh = nullptr;
|
||||
rcPolyMeshDetail *detail_mesh = nullptr;
|
||||
rcContext ctx;
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CONFIGURATION; // step #1
|
||||
|
||||
const float *verts = source_geometry_vertices.ptr();
|
||||
const int nverts = source_geometry_vertices.size() / 3;
|
||||
const int *tris = source_geometry_indices.ptr();
|
||||
const int ntris = source_geometry_indices.size() / 3;
|
||||
|
||||
float bmin[3], bmax[3];
|
||||
rcCalcBounds(verts, nverts, bmin, bmax);
|
||||
|
||||
rcConfig cfg;
|
||||
memset(&cfg, 0, sizeof(cfg));
|
||||
|
||||
cfg.cs = p_navigation_mesh->get_cell_size();
|
||||
cfg.ch = p_navigation_mesh->get_cell_height();
|
||||
if (p_navigation_mesh->get_border_size() > 0.0) {
|
||||
cfg.borderSize = (int)Math::ceil(p_navigation_mesh->get_border_size() / cfg.cs);
|
||||
}
|
||||
cfg.walkableSlopeAngle = p_navigation_mesh->get_agent_max_slope();
|
||||
cfg.walkableHeight = (int)Math::ceil(p_navigation_mesh->get_agent_height() / cfg.ch);
|
||||
cfg.walkableClimb = (int)Math::floor(p_navigation_mesh->get_agent_max_climb() / cfg.ch);
|
||||
cfg.walkableRadius = (int)Math::ceil(p_navigation_mesh->get_agent_radius() / cfg.cs);
|
||||
cfg.maxEdgeLen = (int)(p_navigation_mesh->get_edge_max_length() / p_navigation_mesh->get_cell_size());
|
||||
cfg.maxSimplificationError = p_navigation_mesh->get_edge_max_error();
|
||||
cfg.minRegionArea = (int)(p_navigation_mesh->get_region_min_size() * p_navigation_mesh->get_region_min_size());
|
||||
cfg.mergeRegionArea = (int)(p_navigation_mesh->get_region_merge_size() * p_navigation_mesh->get_region_merge_size());
|
||||
cfg.maxVertsPerPoly = (int)p_navigation_mesh->get_vertices_per_polygon();
|
||||
cfg.detailSampleDist = MAX(p_navigation_mesh->get_cell_size() * p_navigation_mesh->get_detail_sample_distance(), 0.1f);
|
||||
cfg.detailSampleMaxError = p_navigation_mesh->get_cell_height() * p_navigation_mesh->get_detail_sample_max_error();
|
||||
|
||||
if (p_navigation_mesh->get_border_size() > 0.0 && !Math::is_zero_approx(Math::fmod(p_navigation_mesh->get_border_size(), p_navigation_mesh->get_cell_size()))) {
|
||||
WARN_PRINT("Property border_size is ceiled to cell_size voxel units and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.walkableHeight * cfg.ch, p_navigation_mesh->get_agent_height())) {
|
||||
WARN_PRINT("Property agent_height is ceiled to cell_height voxel units and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.walkableClimb * cfg.ch, p_navigation_mesh->get_agent_max_climb())) {
|
||||
WARN_PRINT("Property agent_max_climb is floored to cell_height voxel units and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.walkableRadius * cfg.cs, p_navigation_mesh->get_agent_radius())) {
|
||||
WARN_PRINT("Property agent_radius is ceiled to cell_size voxel units and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.maxEdgeLen * cfg.cs, p_navigation_mesh->get_edge_max_length())) {
|
||||
WARN_PRINT("Property edge_max_length is rounded to cell_size voxel units and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.minRegionArea, p_navigation_mesh->get_region_min_size() * p_navigation_mesh->get_region_min_size())) {
|
||||
WARN_PRINT("Property region_min_size is converted to int and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.mergeRegionArea, p_navigation_mesh->get_region_merge_size() * p_navigation_mesh->get_region_merge_size())) {
|
||||
WARN_PRINT("Property region_merge_size is converted to int and loses precision.");
|
||||
}
|
||||
if (!Math::is_equal_approx((float)cfg.maxVertsPerPoly, p_navigation_mesh->get_vertices_per_polygon())) {
|
||||
WARN_PRINT("Property vertices_per_polygon is converted to int and loses precision.");
|
||||
}
|
||||
if (p_navigation_mesh->get_cell_size() * p_navigation_mesh->get_detail_sample_distance() < 0.1f) {
|
||||
WARN_PRINT("Property detail_sample_distance is clamped to 0.1 world units as the resulting value from multiplying with cell_size is too low.");
|
||||
}
|
||||
|
||||
cfg.bmin[0] = bmin[0];
|
||||
cfg.bmin[1] = bmin[1];
|
||||
cfg.bmin[2] = bmin[2];
|
||||
cfg.bmax[0] = bmax[0];
|
||||
cfg.bmax[1] = bmax[1];
|
||||
cfg.bmax[2] = bmax[2];
|
||||
|
||||
AABB baking_aabb = p_navigation_mesh->get_filter_baking_aabb();
|
||||
if (baking_aabb.has_volume()) {
|
||||
Vector3 baking_aabb_offset = p_navigation_mesh->get_filter_baking_aabb_offset();
|
||||
cfg.bmin[0] = baking_aabb.position[0] + baking_aabb_offset.x;
|
||||
cfg.bmin[1] = baking_aabb.position[1] + baking_aabb_offset.y;
|
||||
cfg.bmin[2] = baking_aabb.position[2] + baking_aabb_offset.z;
|
||||
cfg.bmax[0] = cfg.bmin[0] + baking_aabb.size[0];
|
||||
cfg.bmax[1] = cfg.bmin[1] + baking_aabb.size[1];
|
||||
cfg.bmax[2] = cfg.bmin[2] + baking_aabb.size[2];
|
||||
}
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CALC_GRID_SIZE; // step #2
|
||||
rcCalcGridSize(cfg.bmin, cfg.bmax, cfg.cs, &cfg.width, &cfg.height);
|
||||
|
||||
// ~30000000 seems to be around sweetspot where Editor baking breaks
|
||||
if ((cfg.width * cfg.height) > 30000000 && GLOBAL_GET("navigation/baking/use_crash_prevention_checks")) {
|
||||
ERR_FAIL_MSG("Baking interrupted."
|
||||
"\nNavigationMesh baking process would likely crash the engine."
|
||||
"\nSource geometry is suspiciously big for the current Cell Size and Cell Height in the NavMesh Resource bake settings."
|
||||
"\nIf baking does not crash the engine or fail, the resulting NavigationMesh will create serious pathfinding performance issues."
|
||||
"\nIt is advised to increase Cell Size and/or Cell Height in the NavMesh Resource bake settings or reduce the size / scale of the source geometry."
|
||||
"\nIf you would like to try baking anyway, disable the 'navigation/baking/use_crash_prevention_checks' project setting.");
|
||||
return;
|
||||
}
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CREATE_HEIGHTFIELD; // step #3
|
||||
hf = rcAllocHeightfield();
|
||||
|
||||
ERR_FAIL_NULL(hf);
|
||||
ERR_FAIL_COND(!rcCreateHeightfield(&ctx, *hf, cfg.width, cfg.height, cfg.bmin, cfg.bmax, cfg.cs, cfg.ch));
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_MARK_WALKABLE_TRIANGLES; // step #4
|
||||
{
|
||||
Vector<unsigned char> tri_areas;
|
||||
tri_areas.resize(ntris);
|
||||
|
||||
ERR_FAIL_COND(tri_areas.is_empty());
|
||||
|
||||
memset(tri_areas.ptrw(), 0, ntris * sizeof(unsigned char));
|
||||
rcMarkWalkableTriangles(&ctx, cfg.walkableSlopeAngle, verts, nverts, tris, ntris, tri_areas.ptrw());
|
||||
|
||||
ERR_FAIL_COND(!rcRasterizeTriangles(&ctx, verts, nverts, tris, tri_areas.ptr(), ntris, *hf, cfg.walkableClimb));
|
||||
}
|
||||
|
||||
if (p_navigation_mesh->get_filter_low_hanging_obstacles()) {
|
||||
rcFilterLowHangingWalkableObstacles(&ctx, cfg.walkableClimb, *hf);
|
||||
}
|
||||
if (p_navigation_mesh->get_filter_ledge_spans()) {
|
||||
rcFilterLedgeSpans(&ctx, cfg.walkableHeight, cfg.walkableClimb, *hf);
|
||||
}
|
||||
if (p_navigation_mesh->get_filter_walkable_low_height_spans()) {
|
||||
rcFilterWalkableLowHeightSpans(&ctx, cfg.walkableHeight, *hf);
|
||||
}
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CONSTRUCT_COMPACT_HEIGHTFIELD; // step #5
|
||||
|
||||
chf = rcAllocCompactHeightfield();
|
||||
|
||||
ERR_FAIL_NULL(chf);
|
||||
ERR_FAIL_COND(!rcBuildCompactHeightfield(&ctx, cfg.walkableHeight, cfg.walkableClimb, *hf, *chf));
|
||||
|
||||
rcFreeHeightField(hf);
|
||||
hf = nullptr;
|
||||
|
||||
// Add obstacles to the source geometry. Those will be affected by e.g. agent_radius.
|
||||
if (!projected_obstructions.is_empty()) {
|
||||
for (const NavigationMeshSourceGeometryData3D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
|
||||
if (projected_obstruction.carve) {
|
||||
continue;
|
||||
}
|
||||
if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 3 != 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const float *projected_obstruction_verts = projected_obstruction.vertices.ptr();
|
||||
const int projected_obstruction_nverts = projected_obstruction.vertices.size() / 3;
|
||||
|
||||
rcMarkConvexPolyArea(&ctx, projected_obstruction_verts, projected_obstruction_nverts, projected_obstruction.elevation, projected_obstruction.elevation + projected_obstruction.height, RC_NULL_AREA, *chf);
|
||||
}
|
||||
}
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_ERODE_WALKABLE_AREA; // step #6
|
||||
|
||||
ERR_FAIL_COND(!rcErodeWalkableArea(&ctx, cfg.walkableRadius, *chf));
|
||||
|
||||
// Carve obstacles to the eroded geometry. Those will NOT be affected by e.g. agent_radius because that step is already done.
|
||||
if (!projected_obstructions.is_empty()) {
|
||||
for (const NavigationMeshSourceGeometryData3D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
|
||||
if (!projected_obstruction.carve) {
|
||||
continue;
|
||||
}
|
||||
if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 3 != 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const float *projected_obstruction_verts = projected_obstruction.vertices.ptr();
|
||||
const int projected_obstruction_nverts = projected_obstruction.vertices.size() / 3;
|
||||
|
||||
rcMarkConvexPolyArea(&ctx, projected_obstruction_verts, projected_obstruction_nverts, projected_obstruction.elevation, projected_obstruction.elevation + projected_obstruction.height, RC_NULL_AREA, *chf);
|
||||
}
|
||||
}
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_SAMPLE_PARTITIONING; // step #7
|
||||
|
||||
if (p_navigation_mesh->get_sample_partition_type() == NavigationMesh::SAMPLE_PARTITION_WATERSHED) {
|
||||
ERR_FAIL_COND(!rcBuildDistanceField(&ctx, *chf));
|
||||
ERR_FAIL_COND(!rcBuildRegions(&ctx, *chf, cfg.borderSize, cfg.minRegionArea, cfg.mergeRegionArea));
|
||||
} else if (p_navigation_mesh->get_sample_partition_type() == NavigationMesh::SAMPLE_PARTITION_MONOTONE) {
|
||||
ERR_FAIL_COND(!rcBuildRegionsMonotone(&ctx, *chf, cfg.borderSize, cfg.minRegionArea, cfg.mergeRegionArea));
|
||||
} else {
|
||||
ERR_FAIL_COND(!rcBuildLayerRegions(&ctx, *chf, cfg.borderSize, cfg.minRegionArea));
|
||||
}
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CREATING_CONTOURS; // step #8
|
||||
|
||||
cset = rcAllocContourSet();
|
||||
|
||||
ERR_FAIL_NULL(cset);
|
||||
ERR_FAIL_COND(!rcBuildContours(&ctx, *chf, cfg.maxSimplificationError, cfg.maxEdgeLen, *cset));
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CREATING_POLYMESH; // step #9
|
||||
|
||||
poly_mesh = rcAllocPolyMesh();
|
||||
ERR_FAIL_NULL(poly_mesh);
|
||||
ERR_FAIL_COND(!rcBuildPolyMesh(&ctx, *cset, cfg.maxVertsPerPoly, *poly_mesh));
|
||||
|
||||
detail_mesh = rcAllocPolyMeshDetail();
|
||||
ERR_FAIL_NULL(detail_mesh);
|
||||
ERR_FAIL_COND(!rcBuildPolyMeshDetail(&ctx, *poly_mesh, *chf, cfg.detailSampleDist, cfg.detailSampleMaxError, *detail_mesh));
|
||||
|
||||
rcFreeCompactHeightfield(chf);
|
||||
chf = nullptr;
|
||||
rcFreeContourSet(cset);
|
||||
cset = nullptr;
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CONVERTING_NATIVE_NAVMESH; // step #10
|
||||
|
||||
Vector<Vector3> nav_vertices;
|
||||
Vector<Vector<int>> nav_polygons;
|
||||
|
||||
HashMap<Vector3, int> recast_vertex_to_native_index;
|
||||
LocalVector<int> recast_index_to_native_index;
|
||||
recast_index_to_native_index.resize(detail_mesh->nverts);
|
||||
|
||||
for (int i = 0; i < detail_mesh->nverts; i++) {
|
||||
const float *v = &detail_mesh->verts[i * 3];
|
||||
const Vector3 vertex = Vector3(v[0], v[1], v[2]);
|
||||
int *existing_index_ptr = recast_vertex_to_native_index.getptr(vertex);
|
||||
if (!existing_index_ptr) {
|
||||
int new_index = recast_vertex_to_native_index.size();
|
||||
recast_index_to_native_index[i] = new_index;
|
||||
recast_vertex_to_native_index[vertex] = new_index;
|
||||
nav_vertices.push_back(vertex);
|
||||
} else {
|
||||
recast_index_to_native_index[i] = *existing_index_ptr;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < detail_mesh->nmeshes; i++) {
|
||||
const unsigned int *detail_mesh_m = &detail_mesh->meshes[i * 4];
|
||||
const unsigned int detail_mesh_bverts = detail_mesh_m[0];
|
||||
const unsigned int detail_mesh_m_btris = detail_mesh_m[2];
|
||||
const unsigned int detail_mesh_ntris = detail_mesh_m[3];
|
||||
const unsigned char *detail_mesh_tris = &detail_mesh->tris[detail_mesh_m_btris * 4];
|
||||
for (unsigned int j = 0; j < detail_mesh_ntris; j++) {
|
||||
Vector<int> nav_indices;
|
||||
nav_indices.resize(3);
|
||||
// Polygon order in recast is opposite than godot's
|
||||
int index1 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 0]));
|
||||
int index2 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 2]));
|
||||
int index3 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 1]));
|
||||
|
||||
nav_indices.write[0] = recast_index_to_native_index[index1];
|
||||
nav_indices.write[1] = recast_index_to_native_index[index2];
|
||||
nav_indices.write[2] = recast_index_to_native_index[index3];
|
||||
|
||||
nav_polygons.push_back(nav_indices);
|
||||
}
|
||||
}
|
||||
|
||||
p_navigation_mesh->set_data(nav_vertices, nav_polygons);
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_BAKE_CLEANUP; // step #11
|
||||
|
||||
rcFreePolyMesh(poly_mesh);
|
||||
poly_mesh = nullptr;
|
||||
rcFreePolyMeshDetail(detail_mesh);
|
||||
detail_mesh = nullptr;
|
||||
|
||||
p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_BAKE_FINISHED; // step #12
|
||||
}
|
||||
|
||||
bool NavMeshGenerator3D::generator_emit_callback(const Callable &p_callback) {
|
||||
ERR_FAIL_COND_V(!p_callback.is_valid(), false);
|
||||
|
||||
Callable::CallError ce;
|
||||
Variant result;
|
||||
p_callback.callp(nullptr, 0, result, ce);
|
||||
|
||||
return ce.error == Callable::CallError::CALL_OK;
|
||||
}
|
121
modules/navigation_3d/3d/nav_mesh_generator_3d.h
Normal file
121
modules/navigation_3d/3d/nav_mesh_generator_3d.h
Normal file
@@ -0,0 +1,121 @@
|
||||
/**************************************************************************/
|
||||
/* nav_mesh_generator_3d.h */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "core/object/class_db.h"
|
||||
#include "core/object/worker_thread_pool.h"
|
||||
#include "core/templates/rid_owner.h"
|
||||
#include "servers/navigation_server_3d.h"
|
||||
|
||||
class Node;
|
||||
class NavigationMesh;
|
||||
class NavigationMeshSourceGeometryData3D;
|
||||
|
||||
class NavMeshGenerator3D : public Object {
|
||||
static NavMeshGenerator3D *singleton;
|
||||
|
||||
static Mutex baking_navmesh_mutex;
|
||||
static Mutex generator_task_mutex;
|
||||
|
||||
static RWLock generator_parsers_rwlock;
|
||||
static LocalVector<NavMeshGeometryParser3D *> generator_parsers;
|
||||
|
||||
static bool use_threads;
|
||||
static bool baking_use_multiple_threads;
|
||||
static bool baking_use_high_priority_threads;
|
||||
|
||||
public:
|
||||
enum NavMeshBakeState {
|
||||
BAKE_STATE_NONE,
|
||||
BAKE_STATE_CONFIGURATION,
|
||||
BAKE_STATE_CALC_GRID_SIZE,
|
||||
BAKE_STATE_CREATE_HEIGHTFIELD,
|
||||
BAKE_STATE_MARK_WALKABLE_TRIANGLES,
|
||||
BAKE_STATE_CONSTRUCT_COMPACT_HEIGHTFIELD,
|
||||
BAKE_STATE_ERODE_WALKABLE_AREA,
|
||||
BAKE_STATE_SAMPLE_PARTITIONING,
|
||||
BAKE_STATE_CREATING_CONTOURS,
|
||||
BAKE_STATE_CREATING_POLYMESH,
|
||||
BAKE_STATE_CONVERTING_NATIVE_NAVMESH,
|
||||
BAKE_STATE_BAKE_CLEANUP,
|
||||
BAKE_STATE_BAKE_FINISHED,
|
||||
BAKE_STATE_MAX,
|
||||
};
|
||||
|
||||
private:
|
||||
struct NavMeshGeneratorTask3D {
|
||||
enum TaskStatus {
|
||||
BAKING_STARTED,
|
||||
BAKING_FINISHED,
|
||||
BAKING_FAILED,
|
||||
CALLBACK_DISPATCHED,
|
||||
CALLBACK_FAILED,
|
||||
};
|
||||
|
||||
Ref<NavigationMesh> navigation_mesh;
|
||||
Ref<NavigationMeshSourceGeometryData3D> source_geometry_data;
|
||||
Callable callback;
|
||||
WorkerThreadPool::TaskID thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
|
||||
NavMeshGeneratorTask3D::TaskStatus status = NavMeshGeneratorTask3D::TaskStatus::BAKING_STARTED;
|
||||
|
||||
NavMeshBakeState bake_state = NavMeshBakeState::BAKE_STATE_NONE;
|
||||
};
|
||||
|
||||
static HashMap<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> generator_tasks;
|
||||
|
||||
static void generator_thread_bake(void *p_arg);
|
||||
|
||||
static HashMap<Ref<NavigationMesh>, NavMeshGeneratorTask3D *> baking_navmeshes;
|
||||
|
||||
static void generator_parse_geometry_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node, bool p_recurse_children);
|
||||
static void generator_parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node);
|
||||
static void generator_bake_from_source_geometry_data(NavMeshGeneratorTask3D *p_generator_task);
|
||||
|
||||
static bool generator_emit_callback(const Callable &p_callback);
|
||||
|
||||
public:
|
||||
static NavMeshGenerator3D *get_singleton();
|
||||
|
||||
static void sync();
|
||||
static void cleanup();
|
||||
static void finish();
|
||||
|
||||
static void set_generator_parsers(LocalVector<NavMeshGeometryParser3D *> p_parsers);
|
||||
|
||||
static void parse_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
|
||||
static void bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback = Callable());
|
||||
static void bake_from_source_geometry_data_async(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback = Callable());
|
||||
static bool is_baking(Ref<NavigationMesh> p_navigation_mesh);
|
||||
static String get_baking_state_msg(Ref<NavigationMesh> p_navigation_mesh);
|
||||
|
||||
NavMeshGenerator3D();
|
||||
~NavMeshGenerator3D();
|
||||
};
|
1370
modules/navigation_3d/3d/nav_mesh_queries_3d.cpp
Normal file
1370
modules/navigation_3d/3d/nav_mesh_queries_3d.cpp
Normal file
File diff suppressed because it is too large
Load Diff
162
modules/navigation_3d/3d/nav_mesh_queries_3d.h
Normal file
162
modules/navigation_3d/3d/nav_mesh_queries_3d.h
Normal file
@@ -0,0 +1,162 @@
|
||||
/**************************************************************************/
|
||||
/* nav_mesh_queries_3d.h */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../nav_utils_3d.h"
|
||||
|
||||
#include "core/templates/a_hash_map.h"
|
||||
|
||||
#include "servers/navigation/navigation_globals.h"
|
||||
#include "servers/navigation/navigation_path_query_parameters_3d.h"
|
||||
#include "servers/navigation/navigation_path_query_result_3d.h"
|
||||
#include "servers/navigation/navigation_utilities.h"
|
||||
|
||||
using namespace NavigationUtilities;
|
||||
|
||||
class NavMap3D;
|
||||
struct NavMapIteration3D;
|
||||
|
||||
class NavMeshQueries3D {
|
||||
public:
|
||||
struct PathQuerySlot {
|
||||
LocalVector<Nav3D::NavigationPoly> path_corridor;
|
||||
Heap<Nav3D::NavigationPoly *, Nav3D::NavPolyTravelCostGreaterThan, Nav3D::NavPolyHeapIndexer> traversable_polys;
|
||||
bool in_use = false;
|
||||
uint32_t slot_index = 0;
|
||||
AHashMap<const Nav3D::Polygon *, uint32_t> poly_to_id;
|
||||
};
|
||||
|
||||
struct NavMeshPathQueryTask3D {
|
||||
enum TaskStatus {
|
||||
QUERY_STARTED,
|
||||
QUERY_FINISHED,
|
||||
QUERY_FAILED,
|
||||
CALLBACK_DISPATCHED,
|
||||
CALLBACK_FAILED,
|
||||
};
|
||||
|
||||
// Parameters.
|
||||
Vector3 start_position;
|
||||
Vector3 target_position;
|
||||
uint32_t navigation_layers;
|
||||
BitField<PathMetadataFlags> metadata_flags = PathMetadataFlags::PATH_INCLUDE_ALL;
|
||||
PathfindingAlgorithm pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
|
||||
PathPostProcessing path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
|
||||
bool simplify_path = false;
|
||||
real_t simplify_epsilon = 0.0;
|
||||
|
||||
bool exclude_regions = false;
|
||||
bool include_regions = false;
|
||||
LocalVector<RID> excluded_regions;
|
||||
LocalVector<RID> included_regions;
|
||||
|
||||
float path_return_max_length = 0.0;
|
||||
float path_return_max_radius = 0.0;
|
||||
int path_search_max_polygons = NavigationDefaults3D::path_search_max_polygons;
|
||||
float path_search_max_distance = 0.0;
|
||||
|
||||
// Path building.
|
||||
Vector3 begin_position;
|
||||
Vector3 end_position;
|
||||
const Nav3D::Polygon *begin_polygon = nullptr;
|
||||
const Nav3D::Polygon *end_polygon = nullptr;
|
||||
uint32_t least_cost_id = 0;
|
||||
|
||||
// Map.
|
||||
Vector3 map_up;
|
||||
NavMap3D *map = nullptr;
|
||||
PathQuerySlot *path_query_slot = nullptr;
|
||||
|
||||
// Path points.
|
||||
LocalVector<Vector3> path_points;
|
||||
LocalVector<int32_t> path_meta_point_types;
|
||||
LocalVector<RID> path_meta_point_rids;
|
||||
LocalVector<int64_t> path_meta_point_owners;
|
||||
float path_length = 0.0;
|
||||
|
||||
Ref<NavigationPathQueryParameters3D> query_parameters;
|
||||
Ref<NavigationPathQueryResult3D> query_result;
|
||||
Callable callback;
|
||||
NavMeshPathQueryTask3D::TaskStatus status = NavMeshPathQueryTask3D::TaskStatus::QUERY_STARTED;
|
||||
|
||||
void path_clear() {
|
||||
path_points.clear();
|
||||
path_meta_point_types.clear();
|
||||
path_meta_point_rids.clear();
|
||||
path_meta_point_owners.clear();
|
||||
}
|
||||
|
||||
void path_reverse() {
|
||||
path_points.reverse();
|
||||
path_meta_point_types.reverse();
|
||||
path_meta_point_rids.reverse();
|
||||
path_meta_point_owners.reverse();
|
||||
}
|
||||
};
|
||||
|
||||
static bool emit_callback(const Callable &p_callback);
|
||||
|
||||
static Vector3 polygons_get_random_point(const LocalVector<Nav3D::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly);
|
||||
|
||||
static Vector3 polygons_get_closest_point_to_segment(const LocalVector<Nav3D::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
|
||||
static Vector3 polygons_get_closest_point(const LocalVector<Nav3D::Polygon> &p_polygons, const Vector3 &p_point);
|
||||
static Vector3 polygons_get_closest_point_normal(const LocalVector<Nav3D::Polygon> &p_polygons, const Vector3 &p_point);
|
||||
static Nav3D::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector<Nav3D::Polygon> &p_polygons, const Vector3 &p_point);
|
||||
static RID polygons_get_closest_point_owner(const LocalVector<Nav3D::Polygon> &p_polygons, const Vector3 &p_point);
|
||||
|
||||
static Vector3 map_iteration_get_closest_point_to_segment(const NavMapIteration3D &p_map_iteration, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
|
||||
static Vector3 map_iteration_get_closest_point(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
|
||||
static Vector3 map_iteration_get_closest_point_normal(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
|
||||
static RID map_iteration_get_closest_point_owner(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
|
||||
static Nav3D::ClosestPointQueryResult map_iteration_get_closest_point_info(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
|
||||
static Vector3 map_iteration_get_random_point(const NavMapIteration3D &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly);
|
||||
|
||||
static void map_query_path(NavMap3D *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback);
|
||||
|
||||
static void query_task_map_iteration_get_path(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration);
|
||||
static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const Nav3D::Polygon *p_point_polygon);
|
||||
static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration);
|
||||
static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration);
|
||||
static void _query_task_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task);
|
||||
static void _query_task_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task);
|
||||
static void _query_task_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task);
|
||||
static void _query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const Nav3D::NavigationPoly *from_poly, const Vector3 &p_to_point, const Nav3D::NavigationPoly *p_to_poly);
|
||||
static void _query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task);
|
||||
static bool _query_task_is_connection_owner_usable(const NavMeshPathQueryTask3D &p_query_task, const NavBaseIteration3D *p_owner);
|
||||
static void _query_task_process_path_result_limits(NavMeshPathQueryTask3D &p_query_task);
|
||||
|
||||
static void _query_task_search_polygon_connections(NavMeshPathQueryTask3D &p_query_task, const Nav3D::Connection &p_connection, uint32_t p_least_cost_id, const Nav3D::NavigationPoly &p_least_cost_poly, real_t p_poly_enter_cost, const Vector3 &p_end_point);
|
||||
|
||||
static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<uint32_t> &r_simplified_path_indices);
|
||||
static LocalVector<uint32_t> get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon);
|
||||
|
||||
static float _calculate_path_length(const LocalVector<Vector3> &p_path, uint32_t p_start_index, uint32_t p_end_index);
|
||||
};
|
257
modules/navigation_3d/3d/nav_region_builder_3d.cpp
Normal file
257
modules/navigation_3d/3d/nav_region_builder_3d.cpp
Normal file
@@ -0,0 +1,257 @@
|
||||
/**************************************************************************/
|
||||
/* nav_region_builder_3d.cpp */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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_builder_3d.h"
|
||||
|
||||
#include "../nav_map_3d.h"
|
||||
#include "../nav_region_3d.h"
|
||||
#include "nav_region_iteration_3d.h"
|
||||
|
||||
using namespace Nav3D;
|
||||
|
||||
void NavRegionBuilder3D::build_iteration(NavRegionIterationBuild3D &r_build) {
|
||||
PerformanceData &performance_data = r_build.performance_data;
|
||||
|
||||
performance_data.pm_polygon_count = 0;
|
||||
performance_data.pm_edge_count = 0;
|
||||
performance_data.pm_edge_merge_count = 0;
|
||||
performance_data.pm_edge_connection_count = 0;
|
||||
performance_data.pm_edge_free_count = 0;
|
||||
|
||||
_build_step_process_navmesh_data(r_build);
|
||||
|
||||
_build_step_find_edge_connection_pairs(r_build);
|
||||
|
||||
_build_step_merge_edge_connection_pairs(r_build);
|
||||
|
||||
_build_update_iteration(r_build);
|
||||
}
|
||||
|
||||
void NavRegionBuilder3D::_build_step_process_navmesh_data(NavRegionIterationBuild3D &r_build) {
|
||||
Vector<Vector3> _navmesh_vertices = r_build.navmesh_data.vertices;
|
||||
Vector<Vector<int>> _navmesh_polygons = r_build.navmesh_data.polygons;
|
||||
|
||||
if (_navmesh_vertices.is_empty() || _navmesh_polygons.is_empty()) {
|
||||
return;
|
||||
}
|
||||
|
||||
PerformanceData &performance_data = r_build.performance_data;
|
||||
Ref<NavRegionIteration3D> region_iteration = r_build.region_iteration;
|
||||
|
||||
const Transform3D ®ion_transform = region_iteration->transform;
|
||||
LocalVector<Nav3D::Polygon> &navmesh_polygons = region_iteration->navmesh_polygons;
|
||||
|
||||
const int vertex_count = _navmesh_vertices.size();
|
||||
|
||||
const Vector3 *vertices_ptr = _navmesh_vertices.ptr();
|
||||
const Vector<int> *polygons_ptr = _navmesh_polygons.ptr();
|
||||
|
||||
navmesh_polygons.resize(_navmesh_polygons.size());
|
||||
|
||||
real_t _new_region_surface_area = 0.0;
|
||||
AABB _new_region_bounds;
|
||||
|
||||
bool first_vertex = true;
|
||||
|
||||
for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
|
||||
Polygon &polygon = navmesh_polygons[i];
|
||||
polygon.id = i;
|
||||
polygon.owner = region_iteration.ptr();
|
||||
polygon.surface_area = 0.0;
|
||||
|
||||
Vector<int> polygon_indices = polygons_ptr[i];
|
||||
|
||||
int polygon_size = polygon_indices.size();
|
||||
if (polygon_size < 3) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const int *indices_ptr = polygon_indices.ptr();
|
||||
|
||||
bool polygon_valid = true;
|
||||
|
||||
polygon.vertices.resize(polygon_size);
|
||||
|
||||
{
|
||||
real_t _new_polygon_surface_area = 0.0;
|
||||
|
||||
for (int j(2); j < polygon_size; j++) {
|
||||
const Face3 face = Face3(
|
||||
region_transform.xform(vertices_ptr[indices_ptr[0]]),
|
||||
region_transform.xform(vertices_ptr[indices_ptr[j - 1]]),
|
||||
region_transform.xform(vertices_ptr[indices_ptr[j]]));
|
||||
|
||||
_new_polygon_surface_area += face.get_area();
|
||||
}
|
||||
|
||||
polygon.surface_area = _new_polygon_surface_area;
|
||||
_new_region_surface_area += _new_polygon_surface_area;
|
||||
}
|
||||
|
||||
for (int j(0); j < polygon_size; j++) {
|
||||
int vertex_index = indices_ptr[j];
|
||||
if (vertex_index < 0 || vertex_index >= vertex_count) {
|
||||
polygon_valid = false;
|
||||
break;
|
||||
}
|
||||
|
||||
const Vector3 point_position = region_transform.xform(vertices_ptr[vertex_index]);
|
||||
polygon.vertices[j] = point_position;
|
||||
|
||||
if (first_vertex) {
|
||||
first_vertex = false;
|
||||
_new_region_bounds.position = point_position;
|
||||
} else {
|
||||
_new_region_bounds.expand_to(point_position);
|
||||
}
|
||||
}
|
||||
|
||||
if (!polygon_valid) {
|
||||
polygon.surface_area = 0.0;
|
||||
polygon.vertices.clear();
|
||||
ERR_FAIL_COND_MSG(!polygon_valid, "Corrupted navigation mesh set on region. The indices of a polygon are out of range.");
|
||||
}
|
||||
}
|
||||
|
||||
region_iteration->surface_area = _new_region_surface_area;
|
||||
region_iteration->bounds = _new_region_bounds;
|
||||
|
||||
performance_data.pm_polygon_count = navmesh_polygons.size();
|
||||
}
|
||||
|
||||
Nav3D::PointKey NavRegionBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size) {
|
||||
const int x = static_cast<int>(Math::floor(p_pos.x / p_cell_size.x));
|
||||
const int y = static_cast<int>(Math::floor(p_pos.y / p_cell_size.y));
|
||||
const int z = static_cast<int>(Math::floor(p_pos.z / p_cell_size.z));
|
||||
|
||||
PointKey p;
|
||||
p.key = 0;
|
||||
p.x = x;
|
||||
p.y = y;
|
||||
p.z = z;
|
||||
return p;
|
||||
}
|
||||
|
||||
Nav3D::EdgeKey NavRegionBuilder3D::get_edge_key(const Vector3 &p_vertex1, const Vector3 &p_vertex2, const Vector3 &p_cell_size) {
|
||||
EdgeKey ek(get_point_key(p_vertex1, p_cell_size), get_point_key(p_vertex2, p_cell_size));
|
||||
return ek;
|
||||
}
|
||||
|
||||
void NavRegionBuilder3D::_build_step_find_edge_connection_pairs(NavRegionIterationBuild3D &r_build) {
|
||||
PerformanceData &performance_data = r_build.performance_data;
|
||||
|
||||
const Vector3 &map_cell_size = r_build.map_cell_size;
|
||||
Ref<NavRegionIteration3D> region_iteration = r_build.region_iteration;
|
||||
LocalVector<Nav3D::Polygon> &navmesh_polygons = region_iteration->navmesh_polygons;
|
||||
|
||||
HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
|
||||
connection_pairs_map.clear();
|
||||
|
||||
region_iteration->internal_connections.clear();
|
||||
region_iteration->internal_connections.resize(navmesh_polygons.size());
|
||||
|
||||
region_iteration->external_edges.clear();
|
||||
|
||||
int free_edges_count = 0;
|
||||
|
||||
for (Polygon &poly : region_iteration->navmesh_polygons) {
|
||||
for (uint32_t p = 0; p < poly.vertices.size(); p++) {
|
||||
const int next_point = (p + 1) % poly.vertices.size();
|
||||
const EdgeKey ek = get_edge_key(poly.vertices[p], poly.vertices[next_point], map_cell_size);
|
||||
|
||||
HashMap<EdgeKey, EdgeConnectionPair, EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
|
||||
if (!pair_it) {
|
||||
pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair());
|
||||
performance_data.pm_edge_count += 1;
|
||||
++free_edges_count;
|
||||
}
|
||||
EdgeConnectionPair &pair = pair_it->value;
|
||||
if (pair.size < 2) {
|
||||
// Add the polygon/edge tuple to this key.
|
||||
Connection new_connection;
|
||||
new_connection.polygon = &poly;
|
||||
new_connection.edge = p;
|
||||
new_connection.pathway_start = poly.vertices[p];
|
||||
new_connection.pathway_end = poly.vertices[next_point];
|
||||
|
||||
pair.connections[pair.size] = new_connection;
|
||||
++pair.size;
|
||||
if (pair.size == 2) {
|
||||
--free_edges_count;
|
||||
}
|
||||
|
||||
} else {
|
||||
// The edge is already connected with another edge, skip.
|
||||
ERR_FAIL_COND_MSG(pair.size >= 2, "Navigation region synchronization error. More than 2 edges tried to occupy the same map rasterization space. This is a logical error in the navigation mesh caused by overlap or too densely placed edges.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
performance_data.pm_edge_free_count = free_edges_count;
|
||||
}
|
||||
|
||||
void NavRegionBuilder3D::_build_step_merge_edge_connection_pairs(NavRegionIterationBuild3D &r_build) {
|
||||
PerformanceData &performance_data = r_build.performance_data;
|
||||
|
||||
Ref<NavRegionIteration3D> region_iteration = r_build.region_iteration;
|
||||
|
||||
HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
|
||||
|
||||
for (const KeyValue<EdgeKey, EdgeConnectionPair> &pair_it : connection_pairs_map) {
|
||||
const EdgeConnectionPair &pair = pair_it.value;
|
||||
if (pair.size == 2) {
|
||||
// Connect edge that are shared in different polygons.
|
||||
const Connection &c1 = pair.connections[0];
|
||||
const Connection &c2 = pair.connections[1];
|
||||
region_iteration->internal_connections[c1.polygon->id].push_back(c2);
|
||||
region_iteration->internal_connections[c2.polygon->id].push_back(c1);
|
||||
performance_data.pm_edge_merge_count += 1;
|
||||
|
||||
} else {
|
||||
ERR_FAIL_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size));
|
||||
|
||||
const Connection &connection = pair.connections[0];
|
||||
|
||||
ConnectableEdge ce;
|
||||
ce.ek = pair_it.key;
|
||||
ce.polygon_index = connection.polygon->id;
|
||||
ce.edge = connection.edge;
|
||||
ce.pathway_start = connection.pathway_start;
|
||||
ce.pathway_end = connection.pathway_end;
|
||||
|
||||
region_iteration->external_edges.push_back(ce);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NavRegionBuilder3D::_build_update_iteration(NavRegionIterationBuild3D &r_build) {
|
||||
ERR_FAIL_NULL(r_build.region);
|
||||
// Stub. End of the build.
|
||||
}
|
48
modules/navigation_3d/3d/nav_region_builder_3d.h
Normal file
48
modules/navigation_3d/3d/nav_region_builder_3d.h
Normal file
@@ -0,0 +1,48 @@
|
||||
/**************************************************************************/
|
||||
/* nav_region_builder_3d.h */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../nav_utils_3d.h"
|
||||
|
||||
struct NavRegionIterationBuild3D;
|
||||
|
||||
class NavRegionBuilder3D {
|
||||
static void _build_step_process_navmesh_data(NavRegionIterationBuild3D &r_build);
|
||||
static void _build_step_find_edge_connection_pairs(NavRegionIterationBuild3D &r_build);
|
||||
static void _build_step_merge_edge_connection_pairs(NavRegionIterationBuild3D &r_build);
|
||||
static void _build_update_iteration(NavRegionIterationBuild3D &r_build);
|
||||
|
||||
public:
|
||||
static Nav3D::PointKey get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size);
|
||||
static Nav3D::EdgeKey get_edge_key(const Vector3 &p_vertex1, const Vector3 &p_vertex2, const Vector3 &p_cell_size);
|
||||
|
||||
static void build_iteration(NavRegionIterationBuild3D &r_build);
|
||||
};
|
92
modules/navigation_3d/3d/nav_region_iteration_3d.h
Normal file
92
modules/navigation_3d/3d/nav_region_iteration_3d.h
Normal file
@@ -0,0 +1,92 @@
|
||||
/**************************************************************************/
|
||||
/* nav_region_iteration_3d.h */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../nav_utils_3d.h"
|
||||
#include "nav_base_iteration_3d.h"
|
||||
#include "scene/resources/navigation_mesh.h"
|
||||
|
||||
#include "core/math/aabb.h"
|
||||
|
||||
class NavRegion3D;
|
||||
class NavRegionIteration3D;
|
||||
|
||||
struct NavRegionIterationBuild3D {
|
||||
Nav3D::PerformanceData performance_data;
|
||||
|
||||
NavRegion3D *region = nullptr;
|
||||
|
||||
Vector3 map_cell_size;
|
||||
Transform3D region_transform;
|
||||
|
||||
struct NavMeshData {
|
||||
Vector<Vector3> vertices;
|
||||
Vector<Vector<int>> polygons;
|
||||
|
||||
void clear() {
|
||||
vertices.clear();
|
||||
polygons.clear();
|
||||
}
|
||||
} navmesh_data;
|
||||
|
||||
Ref<NavRegionIteration3D> region_iteration;
|
||||
|
||||
HashMap<Nav3D::EdgeKey, Nav3D::EdgeConnectionPair, Nav3D::EdgeKey> iter_connection_pairs_map;
|
||||
|
||||
void reset() {
|
||||
performance_data.reset();
|
||||
|
||||
navmesh_data.clear();
|
||||
region_iteration = Ref<NavRegionIteration3D>();
|
||||
iter_connection_pairs_map.clear();
|
||||
}
|
||||
};
|
||||
|
||||
class NavRegionIteration3D : public NavBaseIteration3D {
|
||||
GDCLASS(NavRegionIteration3D, NavBaseIteration3D);
|
||||
|
||||
public:
|
||||
Transform3D transform;
|
||||
real_t surface_area = 0.0;
|
||||
AABB bounds;
|
||||
LocalVector<Nav3D::ConnectableEdge> external_edges;
|
||||
|
||||
const Transform3D &get_transform() const { return transform; }
|
||||
real_t get_surface_area() const { return surface_area; }
|
||||
AABB get_bounds() const { return bounds; }
|
||||
const LocalVector<Nav3D::ConnectableEdge> &get_external_edges() const { return external_edges; }
|
||||
|
||||
virtual ~NavRegionIteration3D() override {
|
||||
external_edges.clear();
|
||||
navmesh_polygons.clear();
|
||||
internal_connections.clear();
|
||||
}
|
||||
};
|
74
modules/navigation_3d/3d/navigation_mesh_generator.cpp
Normal file
74
modules/navigation_3d/3d/navigation_mesh_generator.cpp
Normal file
@@ -0,0 +1,74 @@
|
||||
/**************************************************************************/
|
||||
/* navigation_mesh_generator.cpp */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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_generator.h"
|
||||
|
||||
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
|
||||
#include "servers/navigation_server_3d.h"
|
||||
|
||||
NavigationMeshGenerator *NavigationMeshGenerator::singleton = nullptr;
|
||||
|
||||
NavigationMeshGenerator *NavigationMeshGenerator::get_singleton() {
|
||||
return singleton;
|
||||
}
|
||||
|
||||
NavigationMeshGenerator::NavigationMeshGenerator() {
|
||||
singleton = this;
|
||||
}
|
||||
|
||||
NavigationMeshGenerator::~NavigationMeshGenerator() {
|
||||
}
|
||||
|
||||
void NavigationMeshGenerator::bake(const Ref<NavigationMesh> &p_navigation_mesh, Node *p_root_node) {
|
||||
WARN_PRINT_ONCE("NavigationMeshGenerator::bake() is deprecated due to core threading changes. To upgrade existing code, first create a NavigationMeshSourceGeometryData3D resource. Use this resource with method parse_source_geometry_data() to parse the SceneTree for nodes that should contribute to the navigation mesh baking. The SceneTree parsing needs to happen on the main thread. After the parsing is finished use the resource with method bake_from_source_geometry_data() to bake a navigation mesh..");
|
||||
}
|
||||
|
||||
void NavigationMeshGenerator::clear(Ref<NavigationMesh> p_navigation_mesh) {
|
||||
if (p_navigation_mesh.is_valid()) {
|
||||
p_navigation_mesh->clear_polygons();
|
||||
p_navigation_mesh->set_vertices(Vector<Vector3>());
|
||||
}
|
||||
}
|
||||
|
||||
void NavigationMeshGenerator::parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
|
||||
NavigationServer3D::get_singleton()->parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node, p_callback);
|
||||
}
|
||||
|
||||
void NavigationMeshGenerator::bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback) {
|
||||
NavigationServer3D::get_singleton()->bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
|
||||
}
|
||||
|
||||
void NavigationMeshGenerator::_bind_methods() {
|
||||
ClassDB::bind_method(D_METHOD("bake", "navigation_mesh", "root_node"), &NavigationMeshGenerator::bake);
|
||||
ClassDB::bind_method(D_METHOD("clear", "navigation_mesh"), &NavigationMeshGenerator::clear);
|
||||
|
||||
ClassDB::bind_method(D_METHOD("parse_source_geometry_data", "navigation_mesh", "source_geometry_data", "root_node", "callback"), &NavigationMeshGenerator::parse_source_geometry_data, DEFVAL(Callable()));
|
||||
ClassDB::bind_method(D_METHOD("bake_from_source_geometry_data", "navigation_mesh", "source_geometry_data", "callback"), &NavigationMeshGenerator::bake_from_source_geometry_data, DEFVAL(Callable()));
|
||||
}
|
57
modules/navigation_3d/3d/navigation_mesh_generator.h
Normal file
57
modules/navigation_3d/3d/navigation_mesh_generator.h
Normal file
@@ -0,0 +1,57 @@
|
||||
/**************************************************************************/
|
||||
/* navigation_mesh_generator.h */
|
||||
/**************************************************************************/
|
||||
/* This file is part of: */
|
||||
/* GODOT ENGINE */
|
||||
/* https://godotengine.org */
|
||||
/**************************************************************************/
|
||||
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
|
||||
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
|
||||
/* */
|
||||
/* 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. */
|
||||
/**************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "scene/3d/navigation/navigation_region_3d.h"
|
||||
#include "scene/resources/navigation_mesh.h"
|
||||
|
||||
class NavigationMeshSourceGeometryData3D;
|
||||
|
||||
class NavigationMeshGenerator : public Object {
|
||||
GDCLASS(NavigationMeshGenerator, Object);
|
||||
|
||||
static NavigationMeshGenerator *singleton;
|
||||
|
||||
protected:
|
||||
static void _bind_methods();
|
||||
|
||||
public:
|
||||
static NavigationMeshGenerator *get_singleton();
|
||||
|
||||
NavigationMeshGenerator();
|
||||
~NavigationMeshGenerator();
|
||||
|
||||
void bake(const Ref<NavigationMesh> &p_navigation_mesh, Node *p_root_node);
|
||||
void clear(Ref<NavigationMesh> p_navigation_mesh);
|
||||
|
||||
void parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
|
||||
void bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable());
|
||||
};
|
Reference in New Issue
Block a user