mirrors/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2025-01-22 10:32:54 -05:00
Code Issues Projects Releases Packages Wiki Activity

Merge pull request #100882 from smix8/node_navmesh_geo_parsers

Browse source 
Make nodes handle their respective navigation source geometry
This commit is contained in:
Thaddeus Crews 2025-01-16 17:18:01 -06:00
commit 86002e1a3c
No known key found for this signature in database
GPG key ID: 62181B86FE9E5D84
40 changed files with 1528 additions and 1124 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

60
main/main.cpp
View file

@ -164,10 +164,8 @@ static DisplayServer *display_server = nullptr;
static RenderingServer *rendering_server = nullptr;
static TextServerManager *tsman = nullptr;
static ThemeDB *theme_db = nullptr;
static NavigationServer2D *navigation_server_2d = nullptr;
static PhysicsServer2DManager *physics_server_2d_manager = nullptr;
static PhysicsServer2D *physics_server_2d = nullptr;
static NavigationServer3D *navigation_server_3d = nullptr;
#ifndef _3D_DISABLED
static PhysicsServer3DManager *physics_server_3d_manager = nullptr;
static PhysicsServer3D *physics_server_3d = nullptr;
@ -378,44 +376,6 @@ void finalize_display() {
memdelete(display_server);
}
void initialize_navigation_server() {
ERR_FAIL_COND(navigation_server_3d != nullptr);
ERR_FAIL_COND(navigation_server_2d != nullptr);
// Init 3D Navigation Server
navigation_server_3d = NavigationServer3DManager::new_default_server();
// Fall back to dummy if no default server has been registered.
if (!navigation_server_3d) {
navigation_server_3d = memnew(NavigationServer3DDummy);
}
// Should be impossible, but make sure it's not null.
ERR_FAIL_NULL_MSG(navigation_server_3d, "Failed to initialize NavigationServer3D.");
navigation_server_3d->init();
// Init 2D Navigation Server
navigation_server_2d = NavigationServer2DManager::new_default_server();
if (!navigation_server_2d) {
navigation_server_2d = memnew(NavigationServer2DDummy);
}
ERR_FAIL_NULL_MSG(navigation_server_2d, "Failed to initialize NavigationServer2D.");
navigation_server_2d->init();
}
void finalize_navigation_server() {
ERR_FAIL_NULL(navigation_server_3d);
navigation_server_3d->finish();
memdelete(navigation_server_3d);
navigation_server_3d = nullptr;
ERR_FAIL_NULL(navigation_server_2d);
navigation_server_2d->finish();
memdelete(navigation_server_2d);
navigation_server_2d = nullptr;
}
void initialize_theme_db() {
theme_db = memnew(ThemeDB);
}
@ -772,6 +732,9 @@ Error Main::test_setup() {
// Default theme will be initialized later, after modules and ScriptServer are ready.
initialize_theme_db();
NavigationServer3DManager::initialize_server(); // 3D server first because 2D depends on it.
NavigationServer2DManager::initialize_server();
register_scene_types();
register_driver_types();
@ -794,8 +757,6 @@ Error Main::test_setup() {
// Theme needs modules to be initialized so that sub-resources can be loaded.
theme_db->initialize_theme_noproject();
initialize_navigation_server();
ERR_FAIL_COND_V(TextServerManager::get_singleton()->get_interface_count() == 0, ERR_CANT_CREATE);
/* Use one with the most features available. */
@ -856,7 +817,8 @@ void Main::test_cleanup() {
finalize_theme_db();
finalize_navigation_server();
NavigationServer2DManager::finalize_server(); // 2D goes first as it uses the 3D server behind the scene.
NavigationServer3DManager::finalize_server();
GDExtensionManager::get_singleton()->deinitialize_extensions(GDExtension::INITIALIZATION_LEVEL_SERVERS);
uninitialize_modules(MODULE_INITIALIZATION_LEVEL_SERVERS);
@ -3402,6 +3364,11 @@ Error Main::setup2(bool p_show_boot_logo) {
// Default theme will be initialized later, after modules and ScriptServer are ready.
initialize_theme_db();
MAIN_PRINT("Main: Load Navigation");
NavigationServer3DManager::initialize_server(); // 3D server first because 2D depends on it.
NavigationServer2DManager::initialize_server();
register_scene_types();
register_driver_types();
@ -3472,10 +3439,6 @@ Error Main::setup2(bool p_show_boot_logo) {
initialize_physics();
MAIN_PRINT("Main: Load Navigation");
initialize_navigation_server();
register_server_singletons();
// This loads global classes, so it must happen before custom loaders and savers are registered
@ -4716,7 +4679,8 @@ void Main::cleanup(bool p_force) {
finalize_theme_db();
// Before deinitializing server extensions, finalize servers which may be loaded as extensions.
finalize_navigation_server();
NavigationServer2DManager::finalize_server(); // 2D goes first as it uses the 3D server behind the scene.
NavigationServer3DManager::finalize_server();
finalize_physics();
GDExtensionManager::get_singleton()->deinitialize_extensions(GDExtension::INITIALIZATION_LEVEL_SERVERS);

36
modules/csg/csg_shape.cpp
View file

@ -34,9 +34,45 @@
#include "core/io/json.h"
#endif // DEV_ENABLED
#include "core/math/geometry_2d.h"
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
#include "scene/resources/navigation_mesh.h"
#include "servers/navigation_server_3d.h"
#include <manifold/manifold.h>
Callable CSGShape3D::_navmesh_source_geometry_parsing_callback;
RID CSGShape3D::_navmesh_source_geometry_parser;
void CSGShape3D::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer3D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&CSGShape3D::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create();
NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void CSGShape3D::navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
CSGShape3D *csgshape3d = Object::cast_to<CSGShape3D>(p_node);
if (csgshape3d == nullptr) {
return;
}
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS && csgshape3d->is_using_collision() && (csgshape3d->get_collision_layer() & parsed_collision_mask)) || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
Array meshes = csgshape3d->get_meshes();
if (!meshes.is_empty()) {
Ref<Mesh> mesh = meshes[1];
if (mesh.is_valid()) {
p_source_geometry_data->add_mesh(mesh, csgshape3d->get_global_transform());
}
}
}
}
void CSGShape3D::set_use_collision(bool p_enable) {
if (use_collision == p_enable) {
return;

11
modules/csg/csg_shape.h
View file

@ -39,6 +39,9 @@
#include "thirdparty/misc/mikktspace.h"
class NavigationMesh;
class NavigationMeshSourceGeometryData3D;
class CSGShape3D : public GeometryInstance3D {
GDCLASS(CSGShape3D, GeometryInstance3D);
@ -171,6 +174,14 @@ public:
virtual Ref<TriangleMesh> generate_triangle_mesh() const override;
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
CSGShape3D();
~CSGShape3D();
};

1
modules/csg/register_types.cpp
View file

@ -47,6 +47,7 @@ void initialize_csg_module(ModuleInitializationLevel p_level) {
GDREGISTER_CLASS(CSGTorus3D);
GDREGISTER_CLASS(CSGPolygon3D);
GDREGISTER_CLASS(CSGCombiner3D);
CSGShape3D::navmesh_parse_init();
}
#ifdef TOOLS_ENABLED
if (p_level == MODULE_INITIALIZATION_LEVEL_EDITOR) {

151
modules/gridmap/grid_map.cpp
View file

@ -31,12 +31,26 @@
#include "grid_map.h"
#include "core/io/marshalls.h"
#include "core/math/convex_hull.h"
#include "scene/resources/3d/box_shape_3d.h"
#include "scene/resources/3d/capsule_shape_3d.h"
#include "scene/resources/3d/concave_polygon_shape_3d.h"
#include "scene/resources/3d/convex_polygon_shape_3d.h"
#include "scene/resources/3d/cylinder_shape_3d.h"
#include "scene/resources/3d/height_map_shape_3d.h"
#include "scene/resources/3d/mesh_library.h"
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
#include "scene/resources/3d/primitive_meshes.h"
#include "scene/resources/3d/shape_3d.h"
#include "scene/resources/3d/sphere_shape_3d.h"
#include "scene/resources/physics_material.h"
#include "scene/resources/surface_tool.h"
#include "servers/navigation_server_3d.h"
#include "servers/rendering_server.h"
Callable GridMap::_navmesh_source_geometry_parsing_callback;
RID GridMap::_navmesh_source_geometry_parser;
bool GridMap::_set(const StringName &p_name, const Variant &p_value) {
String name = p_name;
@ -1336,6 +1350,143 @@ GridMap::GridMap() {
#endif // DEBUG_ENABLED
}
void GridMap::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer3D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&GridMap::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create();
NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void GridMap::navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
GridMap *gridmap = Object::cast_to<GridMap>(p_node);
if (gridmap == nullptr) {
return;
}
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
Array meshes = gridmap->get_meshes();
Transform3D xform = gridmap->get_global_transform();
for (int i = 0; i < meshes.size(); i += 2) {
Ref<Mesh> mesh = meshes[i + 1];
if (mesh.is_valid()) {
p_source_geometry_data->add_mesh(mesh, xform * (Transform3D)meshes[i]);
}
}
}
else if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (gridmap->get_collision_layer() & parsed_collision_mask)) {
Array shapes = gridmap->get_collision_shapes();
for (int i = 0; i < shapes.size(); i += 2) {
RID shape = shapes[i + 1];
PhysicsServer3D::ShapeType type = PhysicsServer3D::get_singleton()->shape_get_type(shape);
Variant data = PhysicsServer3D::get_singleton()->shape_get_data(shape);
switch (type) {
case PhysicsServer3D::SHAPE_SPHERE: {
real_t radius = data;
Array arr;
arr.resize(RS::ARRAY_MAX);
SphereMesh::create_mesh_array(arr, radius, radius * 2.0);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_BOX: {
Vector3 extents = data;
Array arr;
arr.resize(RS::ARRAY_MAX);
BoxMesh::create_mesh_array(arr, extents * 2.0);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_CAPSULE: {
Dictionary dict = data;
real_t radius = dict["radius"];
real_t height = dict["height"];
Array arr;
arr.resize(RS::ARRAY_MAX);
CapsuleMesh::create_mesh_array(arr, radius, height);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_CYLINDER: {
Dictionary dict = data;
real_t radius = dict["radius"];
real_t height = dict["height"];
Array arr;
arr.resize(RS::ARRAY_MAX);
CylinderMesh::create_mesh_array(arr, radius, radius, height);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_CONVEX_POLYGON: {
PackedVector3Array vertices = data;
Geometry3D::MeshData md;
Error err = ConvexHullComputer::convex_hull(vertices, md);
if (err == OK) {
PackedVector3Array faces;
for (const Geometry3D::MeshData::Face &face : md.faces) {
for (uint32_t k = 2; k < face.indices.size(); ++k) {
faces.push_back(md.vertices[face.indices[0]]);
faces.push_back(md.vertices[face.indices[k - 1]]);
faces.push_back(md.vertices[face.indices[k]]);
}
}
p_source_geometry_data->add_faces(faces, shapes[i]);
}
} break;
case PhysicsServer3D::SHAPE_CONCAVE_POLYGON: {
Dictionary dict = data;
PackedVector3Array faces = Variant(dict["faces"]);
p_source_geometry_data->add_faces(faces, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_HEIGHTMAP: {
Dictionary dict = data;
///< dict( int:"width", int:"depth",float:"cell_size", float_array:"heights"
int heightmap_depth = dict["depth"];
int heightmap_width = dict["width"];
if (heightmap_depth >= 2 && heightmap_width >= 2) {
const Vector<real_t> &map_data = dict["heights"];
Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1);
Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5;
Vector<Vector3> vertex_array;
vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6);
Vector3 *vertex_array_ptrw = vertex_array.ptrw();
const real_t *map_data_ptr = map_data.ptr();
int vertex_index = 0;
for (int d = 0; d < heightmap_depth - 1; d++) {
for (int w = 0; w < heightmap_width - 1; w++) {
vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d);
vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1);
vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_index += 6;
}
}
if (vertex_array.size() > 0) {
p_source_geometry_data->add_faces(vertex_array, shapes[i]);
}
}
} break;
default: {
WARN_PRINT("Unsupported collision shape type.");
} break;
}
}
}
}
#ifdef DEBUG_ENABLED
void GridMap::_update_navigation_debug_edge_connections() {
if (bake_navigation) {

10
modules/gridmap/grid_map.h
View file

@ -38,6 +38,8 @@
//heh heh, godotsphir!! this shares no code and the design is completely different with previous projects i've done..
//should scale better with hardware that supports instancing
class NavigationMesh;
class NavigationMeshSourceGeometryData3D;
class PhysicsMaterial;
class GridMap : public Node3D {
@ -300,6 +302,14 @@ public:
Array get_bake_meshes();
RID get_bake_mesh_instance(int p_idx);
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
GridMap();
~GridMap();
};

1
modules/gridmap/register_types.cpp
View file

@ -43,6 +43,7 @@
void initialize_gridmap_module(ModuleInitializationLevel p_level) {
if (p_level == MODULE_INITIALIZATION_LEVEL_SCENE) {
GDREGISTER_CLASS(GridMap);
GridMap::navmesh_parse_init();
}
#ifdef TOOLS_ENABLED
if (p_level == MODULE_INITIALIZATION_LEVEL_EDITOR) {

44
modules/navigation/2d/godot_navigation_server_2d.cpp
View file

@ -169,6 +169,8 @@ void GodotNavigationServer2D::init() {
#ifdef CLIPPER2_ENABLED
navmesh_generator_2d = memnew(NavMeshGenerator2D);
ERR_FAIL_NULL_MSG(navmesh_generator_2d, "Failed to init NavMeshGenerator2D.");
RWLockRead read_lock(geometry_parser_rwlock);
navmesh_generator_2d->set_generator_parsers(generator_parsers);
#endif // CLIPPER2_ENABLED
}
@ -411,12 +413,19 @@ void FORWARD_2(agent_set_paused, RID, p_agent, bool, p_paused, rid_to_rid, bool_
bool FORWARD_1_C(agent_get_paused, RID, p_agent, rid_to_rid);
void GodotNavigationServer2D::free(RID p_object) {
#ifdef CLIPPER2_ENABLED
if (navmesh_generator_2d && navmesh_generator_2d->owns(p_object)) {
navmesh_generator_2d->free(p_object);
if (geometry_parser_owner.owns(p_object)) {
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_object);
ERR_FAIL_NULL(parser);
generator_parsers.erase(parser);
#ifndef CLIPPER2_ENABLED
NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
#endif
geometry_parser_owner.free(parser->self);
return;
}
#endif // CLIPPER2_ENABLED
NavigationServer3D::get_singleton()->free(p_object);
}
@ -517,18 +526,25 @@ void GodotNavigationServer2D::query_path(const Ref<NavigationPathQueryParameters
}
RID GodotNavigationServer2D::source_geometry_parser_create() {
RWLockWrite write_lock(geometry_parser_rwlock);
RID rid = geometry_parser_owner.make_rid();
NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(rid);
parser->self = rid;
generator_parsers.push_back(parser);
#ifdef CLIPPER2_ENABLED
if (navmesh_generator_2d) {
return navmesh_generator_2d->source_geometry_parser_create();
}
#endif // CLIPPER2_ENABLED
return RID();
NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
#endif
return rid;
}
void GodotNavigationServer2D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
#ifdef CLIPPER2_ENABLED
if (navmesh_generator_2d) {
navmesh_generator_2d->source_geometry_parser_set_callback(p_parser, p_callback);
}
#endif // CLIPPER2_ENABLED
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_parser);
ERR_FAIL_NULL(parser);
parser->callback = p_callback;
}

559
modules/navigation/2d/nav_mesh_generator_2d.cpp
View file

@ -33,19 +33,8 @@
#include "nav_mesh_generator_2d.h"
#include "core/config/project_settings.h"
#include "scene/2d/mesh_instance_2d.h"
#include "scene/2d/multimesh_instance_2d.h"
#include "scene/2d/navigation_obstacle_2d.h"
#include "scene/2d/physics/static_body_2d.h"
#include "scene/2d/polygon_2d.h"
#include "scene/2d/tile_map.h"
#include "scene/resources/2d/capsule_shape_2d.h"
#include "scene/resources/2d/circle_shape_2d.h"
#include "scene/resources/2d/concave_polygon_shape_2d.h"
#include "scene/resources/2d/convex_polygon_shape_2d.h"
#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h"
#include "scene/resources/2d/navigation_polygon.h"
#include "scene/resources/2d/rectangle_shape_2d.h"
#include "thirdparty/clipper2/include/clipper2/clipper.h"
#include "thirdparty/misc/polypartition.h"
@ -53,14 +42,13 @@
NavMeshGenerator2D *NavMeshGenerator2D::singleton = nullptr;
Mutex NavMeshGenerator2D::baking_navmesh_mutex;
Mutex NavMeshGenerator2D::generator_task_mutex;
RWLock NavMeshGenerator2D::generator_rid_rwlock;
RWLock NavMeshGenerator2D::generator_parsers_rwlock;
bool NavMeshGenerator2D::use_threads = true;
bool NavMeshGenerator2D::baking_use_multiple_threads = true;
bool NavMeshGenerator2D::baking_use_high_priority_threads = true;
HashSet<Ref<NavigationPolygon>> NavMeshGenerator2D::baking_navmeshes;
HashMap<WorkerThreadPool::TaskID, NavMeshGenerator2D::NavMeshGeneratorTask2D *> NavMeshGenerator2D::generator_tasks;
RID_Owner<NavMeshGenerator2D::NavMeshGeometryParser2D> NavMeshGenerator2D::generator_parser_owner;
LocalVector<NavMeshGenerator2D::NavMeshGeometryParser2D *> NavMeshGenerator2D::generator_parsers;
LocalVector<NavMeshGeometryParser2D *> NavMeshGenerator2D::generator_parsers;
NavMeshGenerator2D *NavMeshGenerator2D::get_singleton() {
return singleton;
@ -129,12 +117,9 @@ void NavMeshGenerator2D::cleanup() {
}
generator_tasks.clear();
generator_rid_rwlock.write_lock();
for (NavMeshGeometryParser2D *parser : generator_parsers) {
generator_parser_owner.free(parser->self);
}
generator_parsers_rwlock.write_lock();
generator_parsers.clear();
generator_rid_rwlock.write_unlock();
generator_parsers_rwlock.write_unlock();
}
}
@ -234,510 +219,25 @@ void NavMeshGenerator2D::generator_thread_bake(void *p_arg) {
}
void NavMeshGenerator2D::generator_parse_geometry_node(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node, bool p_recurse_children) {
generator_parse_meshinstance2d_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_parse_multimeshinstance2d_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_parse_polygon2d_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_parse_staticbody2d_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_parse_tile_map_layer_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_parse_navigationobstacle_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_rid_rwlock.read_lock();
generator_parsers_rwlock.read_lock();
for (const NavMeshGeometryParser2D *parser : generator_parsers) {
if (!parser->callback.is_valid()) {
continue;
}
parser->callback.call(p_navigation_mesh, p_source_geometry_data, p_node);
}
generator_rid_rwlock.read_unlock();
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);
}
} else if (Object::cast_to<TileMap>(p_node)) {
// Special case for TileMap, so that internal layer get parsed even if p_recurse_children is false.
for (int i = 0; i < p_node->get_child_count(); i++) {
TileMapLayer *tile_map_layer = Object::cast_to<TileMapLayer>(p_node->get_child(i));
if (tile_map_layer && tile_map_layer->get_index_in_tile_map() >= 0) {
generator_parse_tile_map_layer_node(p_navigation_mesh, p_source_geometry_data, tile_map_layer);
}
}
}
}
void NavMeshGenerator2D::generator_parse_meshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
MeshInstance2D *mesh_instance = Object::cast_to<MeshInstance2D>(p_node);
if (mesh_instance == nullptr) {
return;
}
NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) {
return;
}
Ref<Mesh> mesh = mesh_instance->get_mesh();
if (mesh.is_null()) {
return;
}
const Transform2D mesh_instance_xform = p_source_geometry_data->root_node_transform * mesh_instance->get_global_transform();
using namespace Clipper2Lib;
PathsD subject_paths, dummy_clip_paths;
for (int i = 0; i < mesh->get_surface_count(); i++) {
if (mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) {
continue;
}
if (!(mesh->surface_get_format(i) & Mesh::ARRAY_FLAG_USE_2D_VERTICES)) {
continue;
}
PathD subject_path;
int index_count = 0;
if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
index_count = mesh->surface_get_array_index_len(i);
} else {
index_count = mesh->surface_get_array_len(i);
}
ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0));
Array a = mesh->surface_get_arrays(i);
Vector<Vector2> mesh_vertices = a[Mesh::ARRAY_VERTEX];
if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
Vector<int> mesh_indices = a[Mesh::ARRAY_INDEX];
for (int vertex_index : mesh_indices) {
const Vector2 &vertex = mesh_vertices[vertex_index];
const PointD &point = PointD(vertex.x, vertex.y);
subject_path.push_back(point);
}
} else {
for (const Vector2 &vertex : mesh_vertices) {
const PointD &point = PointD(vertex.x, vertex.y);
subject_path.push_back(point);
}
}
subject_paths.push_back(subject_path);
}
PathsD path_solution;
path_solution = Union(subject_paths, dummy_clip_paths, FillRule::NonZero);
//path_solution = RamerDouglasPeucker(path_solution, 0.025);
Vector<Vector<Vector2>> polypaths;
for (const PathD &scaled_path : path_solution) {
Vector<Vector2> shape_outline;
for (const PointD &scaled_point : scaled_path) {
shape_outline.push_back(Point2(static_cast<real_t>(scaled_point.x), static_cast<real_t>(scaled_point.y)));
}
for (int i = 0; i < shape_outline.size(); i++) {
shape_outline.write[i] = mesh_instance_xform.xform(shape_outline[i]);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
}
void NavMeshGenerator2D::generator_parse_multimeshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
MultiMeshInstance2D *multimesh_instance = Object::cast_to<MultiMeshInstance2D>(p_node);
if (multimesh_instance == nullptr) {
return;
}
NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) {
return;
}
Ref<MultiMesh> multimesh = multimesh_instance->get_multimesh();
if (!(multimesh.is_valid() && multimesh->get_transform_format() == MultiMesh::TRANSFORM_2D)) {
return;
}
Ref<Mesh> mesh = multimesh->get_mesh();
if (mesh.is_null()) {
return;
}
using namespace Clipper2Lib;
PathsD mesh_subject_paths, dummy_clip_paths;
for (int i = 0; i < mesh->get_surface_count(); i++) {
if (mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) {
continue;
}
if (!(mesh->surface_get_format(i) & Mesh::ARRAY_FLAG_USE_2D_VERTICES)) {
continue;
}
PathD subject_path;
int index_count = 0;
if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
index_count = mesh->surface_get_array_index_len(i);
} else {
index_count = mesh->surface_get_array_len(i);
}
ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0));
Array a = mesh->surface_get_arrays(i);
Vector<Vector2> mesh_vertices = a[Mesh::ARRAY_VERTEX];
if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
Vector<int> mesh_indices = a[Mesh::ARRAY_INDEX];
for (int vertex_index : mesh_indices) {
const Vector2 &vertex = mesh_vertices[vertex_index];
const PointD &point = PointD(vertex.x, vertex.y);
subject_path.push_back(point);
}
} else {
for (const Vector2 &vertex : mesh_vertices) {
const PointD &point = PointD(vertex.x, vertex.y);
subject_path.push_back(point);
}
}
mesh_subject_paths.push_back(subject_path);
}
PathsD mesh_path_solution = Union(mesh_subject_paths, dummy_clip_paths, FillRule::NonZero);
//path_solution = RamerDouglasPeucker(path_solution, 0.025);
int multimesh_instance_count = multimesh->get_visible_instance_count();
if (multimesh_instance_count == -1) {
multimesh_instance_count = multimesh->get_instance_count();
}
const Transform2D multimesh_instance_xform = p_source_geometry_data->root_node_transform * multimesh_instance->get_global_transform();
for (int i = 0; i < multimesh_instance_count; i++) {
const Transform2D multimesh_instance_mesh_instance_xform = multimesh_instance_xform * multimesh->get_instance_transform_2d(i);
for (const PathD &mesh_path : mesh_path_solution) {
Vector<Vector2> shape_outline;
for (const PointD &mesh_path_point : mesh_path) {
shape_outline.push_back(Point2(static_cast<real_t>(mesh_path_point.x), static_cast<real_t>(mesh_path_point.y)));
}
for (int j = 0; j < shape_outline.size(); j++) {
shape_outline.write[j] = multimesh_instance_mesh_instance_xform.xform(shape_outline[j]);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
}
}
void NavMeshGenerator2D::generator_parse_polygon2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
Polygon2D *polygon_2d = Object::cast_to<Polygon2D>(p_node);
if (polygon_2d == nullptr) {
return;
}
NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH) {
const Transform2D polygon_2d_xform = p_source_geometry_data->root_node_transform * polygon_2d->get_global_transform();
Vector<Vector2> shape_outline = polygon_2d->get_polygon();
for (int i = 0; i < shape_outline.size(); i++) {
shape_outline.write[i] = polygon_2d_xform.xform(shape_outline[i]);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
}
void NavMeshGenerator2D::generator_parse_staticbody2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
StaticBody2D *static_body = Object::cast_to<StaticBody2D>(p_node);
if (static_body == nullptr) {
return;
}
NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) {
return;
}
uint32_t parsed_collision_mask = p_navigation_mesh->get_parsed_collision_mask();
if (!(static_body->get_collision_layer() & parsed_collision_mask)) {
return;
}
List<uint32_t> shape_owners;
static_body->get_shape_owners(&shape_owners);
for (uint32_t shape_owner : shape_owners) {
if (static_body->is_shape_owner_disabled(shape_owner)) {
continue;
}
const int shape_count = static_body->shape_owner_get_shape_count(shape_owner);
for (int shape_index = 0; shape_index < shape_count; shape_index++) {
Ref<Shape2D> s = static_body->shape_owner_get_shape(shape_owner, shape_index);
if (s.is_null()) {
continue;
}
const Transform2D static_body_xform = p_source_geometry_data->root_node_transform * static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner);
RectangleShape2D *rectangle_shape = Object::cast_to<RectangleShape2D>(*s);
if (rectangle_shape) {
Vector<Vector2> shape_outline;
const Vector2 &rectangle_size = rectangle_shape->get_size();
shape_outline.resize(5);
shape_outline.write[0] = static_body_xform.xform(-rectangle_size * 0.5);
shape_outline.write[1] = static_body_xform.xform(Vector2(rectangle_size.x, -rectangle_size.y) * 0.5);
shape_outline.write[2] = static_body_xform.xform(rectangle_size * 0.5);
shape_outline.write[3] = static_body_xform.xform(Vector2(-rectangle_size.x, rectangle_size.y) * 0.5);
shape_outline.write[4] = static_body_xform.xform(-rectangle_size * 0.5);
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
CapsuleShape2D *capsule_shape = Object::cast_to<CapsuleShape2D>(*s);
if (capsule_shape) {
const real_t capsule_height = capsule_shape->get_height();
const real_t capsule_radius = capsule_shape->get_radius();
Vector<Vector2> shape_outline;
const real_t turn_step = Math_TAU / 12.0;
shape_outline.resize(14);
int shape_outline_inx = 0;
for (int i = 0; i < 12; i++) {
Vector2 ofs = Vector2(0, (i > 3 && i <= 9) ? -capsule_height * 0.5 + capsule_radius : capsule_height * 0.5 - capsule_radius);
shape_outline.write[shape_outline_inx] = static_body_xform.xform(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * capsule_radius + ofs);
shape_outline_inx += 1;
if (i == 3 || i == 9) {
shape_outline.write[shape_outline_inx] = static_body_xform.xform(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * capsule_radius - ofs);
shape_outline_inx += 1;
}
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
CircleShape2D *circle_shape = Object::cast_to<CircleShape2D>(*s);
if (circle_shape) {
const real_t circle_radius = circle_shape->get_radius();
Vector<Vector2> shape_outline;
int circle_edge_count = 12;
shape_outline.resize(circle_edge_count);
const real_t turn_step = Math_TAU / real_t(circle_edge_count);
for (int i = 0; i < circle_edge_count; i++) {
shape_outline.write[i] = static_body_xform.xform(Vector2(Math::cos(i * turn_step), Math::sin(i * turn_step)) * circle_radius);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
ConcavePolygonShape2D *concave_polygon_shape = Object::cast_to<ConcavePolygonShape2D>(*s);
if (concave_polygon_shape) {
Vector<Vector2> shape_outline = concave_polygon_shape->get_segments();
for (int i = 0; i < shape_outline.size(); i++) {
shape_outline.write[i] = static_body_xform.xform(shape_outline[i]);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
ConvexPolygonShape2D *convex_polygon_shape = Object::cast_to<ConvexPolygonShape2D>(*s);
if (convex_polygon_shape) {
Vector<Vector2> shape_outline = convex_polygon_shape->get_points();
for (int i = 0; i < shape_outline.size(); i++) {
shape_outline.write[i] = static_body_xform.xform(shape_outline[i]);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
}
}
}
void NavMeshGenerator2D::generator_parse_tile_map_layer_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
TileMapLayer *tile_map_layer = Object::cast_to<TileMapLayer>(p_node);
if (tile_map_layer == nullptr) {
return;
}
Ref<TileSet> tile_set = tile_map_layer->get_tile_set();
if (tile_set.is_null()) {
return;
}
int physics_layers_count = tile_set->get_physics_layers_count();
int navigation_layers_count = tile_set->get_navigation_layers_count();
if (physics_layers_count <= 0 && navigation_layers_count <= 0) {
return;
}
NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
uint32_t parsed_collision_mask = p_navigation_mesh->get_parsed_collision_mask();
const Transform2D tilemap_xform = p_source_geometry_data->root_node_transform * tile_map_layer->get_global_transform();
TypedArray<Vector2i> used_cells = tile_map_layer->get_used_cells();
for (int used_cell_index = 0; used_cell_index < used_cells.size(); used_cell_index++) {
const Vector2i &cell = used_cells[used_cell_index];
const TileData *tile_data = tile_map_layer->get_cell_tile_data(cell);
if (tile_data == nullptr) {
continue;
}
// Transform flags.
const int alternative_id = tile_map_layer->get_cell_alternative_tile(cell);
bool flip_h = (alternative_id & TileSetAtlasSource::TRANSFORM_FLIP_H);
bool flip_v = (alternative_id & TileSetAtlasSource::TRANSFORM_FLIP_V);
bool transpose = (alternative_id & TileSetAtlasSource::TRANSFORM_TRANSPOSE);
Transform2D tile_transform;
tile_transform.set_origin(tile_map_layer->map_to_local(cell));
const Transform2D tile_transform_offset = tilemap_xform * tile_transform;
// Parse traversable polygons.
for (int navigation_layer = 0; navigation_layer < navigation_layers_count; navigation_layer++) {
Ref<NavigationPolygon> navigation_polygon = tile_data->get_navigation_polygon(navigation_layer, flip_h, flip_v, transpose);
if (navigation_polygon.is_valid()) {
for (int outline_index = 0; outline_index < navigation_polygon->get_outline_count(); outline_index++) {
const Vector<Vector2> &navigation_polygon_outline = navigation_polygon->get_outline(outline_index);
if (navigation_polygon_outline.is_empty()) {
continue;
}
Vector<Vector2> traversable_outline;
traversable_outline.resize(navigation_polygon_outline.size());
const Vector2 *navigation_polygon_outline_ptr = navigation_polygon_outline.ptr();
Vector2 *traversable_outline_ptrw = traversable_outline.ptrw();
for (int traversable_outline_index = 0; traversable_outline_index < traversable_outline.size(); traversable_outline_index++) {
traversable_outline_ptrw[traversable_outline_index] = tile_transform_offset.xform(navigation_polygon_outline_ptr[traversable_outline_index]);
}
p_source_geometry_data->_add_traversable_outline(traversable_outline);
}
}
}
// Parse obstacles.
for (int physics_layer = 0; physics_layer < physics_layers_count; physics_layer++) {
if ((parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH) &&
(tile_set->get_physics_layer_collision_layer(physics_layer) & parsed_collision_mask)) {
for (int collision_polygon_index = 0; collision_polygon_index < tile_data->get_collision_polygons_count(physics_layer); collision_polygon_index++) {
PackedVector2Array collision_polygon_points = tile_data->get_collision_polygon_points(physics_layer, collision_polygon_index);
if (collision_polygon_points.is_empty()) {
continue;
}
if (flip_h || flip_v || transpose) {
collision_polygon_points = TileData::get_transformed_vertices(collision_polygon_points, flip_h, flip_v, transpose);
}
Vector<Vector2> obstruction_outline;
obstruction_outline.resize(collision_polygon_points.size());
const Vector2 *collision_polygon_points_ptr = collision_polygon_points.ptr();
Vector2 *obstruction_outline_ptrw = obstruction_outline.ptrw();
for (int obstruction_outline_index = 0; obstruction_outline_index < obstruction_outline.size(); obstruction_outline_index++) {
obstruction_outline_ptrw[obstruction_outline_index] = tile_transform_offset.xform(collision_polygon_points_ptr[obstruction_outline_index]);
}
p_source_geometry_data->_add_obstruction_outline(obstruction_outline);
}
}
}
}
}
void NavMeshGenerator2D::generator_parse_navigationobstacle_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
NavigationObstacle2D *obstacle = Object::cast_to<NavigationObstacle2D>(p_node);
if (obstacle == nullptr) {
return;
}
if (!obstacle->get_affect_navigation_mesh()) {
return;
}
const Vector2 safe_scale = obstacle->get_global_scale().abs().maxf(0.001);
const float obstacle_radius = obstacle->get_radius();
if (obstacle_radius > 0.0) {
// Radius defined obstacle should be uniformly scaled from obstacle basis max scale axis.
const float scaling_max_value = safe_scale[safe_scale.max_axis_index()];
const Vector2 uniform_max_scale = Vector2(scaling_max_value, scaling_max_value);
const Transform2D obstacle_circle_transform = p_source_geometry_data->root_node_transform * Transform2D(obstacle->get_global_rotation(), uniform_max_scale, 0.0, obstacle->get_global_position());
Vector<Vector2> obstruction_circle_vertices;
// The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding.
// Without, navigation paths can go directly through the middle of the obstacle and conflict with the avoidance to get agents stuck.
// No place for excessive "round" detail here. Every additional edge adds a high cost for something that needs to be quick, not pretty.
static const int circle_points = 12;
obstruction_circle_vertices.resize(circle_points);
Vector2 *circle_vertices_ptrw = obstruction_circle_vertices.ptrw();
const real_t circle_point_step = Math_TAU / circle_points;
for (int i = 0; i < circle_points; i++) {
const float angle = i * circle_point_step;
circle_vertices_ptrw[i] = obstacle_circle_transform.xform(Vector2(Math::cos(angle) * obstacle_radius, Math::sin(angle) * obstacle_radius));
}
p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, obstacle->get_carve_navigation_mesh());
}
// Obstacles are projected to the xz-plane, so only rotation around the y-axis can be taken into account.
const Transform2D node_xform = p_source_geometry_data->root_node_transform * obstacle->get_global_transform();
const Vector<Vector2> &obstacle_vertices = obstacle->get_vertices();
if (obstacle_vertices.is_empty()) {
return;
}
Vector<Vector2> obstruction_shape_vertices;
obstruction_shape_vertices.resize(obstacle_vertices.size());
const Vector2 *obstacle_vertices_ptr = obstacle_vertices.ptr();
Vector2 *obstruction_shape_vertices_ptrw = obstruction_shape_vertices.ptrw();
for (int i = 0; i < obstacle_vertices.size(); i++) {
obstruction_shape_vertices_ptrw[i] = node_xform.xform(obstacle_vertices_ptr[i]);
}
p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, obstacle->get_carve_navigation_mesh());
void NavMeshGenerator2D::set_generator_parsers(LocalVector<NavMeshGeometryParser2D *> p_parsers) {
RWLockWrite write_lock(generator_parsers_rwlock);
generator_parsers = p_parsers;
}
void NavMeshGenerator2D::generator_parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node) {
@ -801,47 +301,6 @@ bool NavMeshGenerator2D::generator_emit_callback(const Callable &p_callback) {
return ce.error == Callable::CallError::CALL_OK;
}
RID NavMeshGenerator2D::source_geometry_parser_create() {
RWLockWrite write_lock(generator_rid_rwlock);
RID rid = generator_parser_owner.make_rid();
NavMeshGeometryParser2D *parser = generator_parser_owner.get_or_null(rid);
parser->self = rid;
generator_parsers.push_back(parser);
return rid;
}
void NavMeshGenerator2D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
RWLockWrite write_lock(generator_rid_rwlock);
NavMeshGeometryParser2D *parser = generator_parser_owner.get_or_null(p_parser);
ERR_FAIL_NULL(parser);
parser->callback = p_callback;
}
bool NavMeshGenerator2D::owns(RID p_object) {
RWLockRead read_lock(generator_rid_rwlock);
return generator_parser_owner.owns(p_object);
}
void NavMeshGenerator2D::free(RID p_object) {
RWLockWrite write_lock(generator_rid_rwlock);
if (generator_parser_owner.owns(p_object)) {
NavMeshGeometryParser2D *parser = generator_parser_owner.get_or_null(p_object);
generator_parsers.erase(parser);
generator_parser_owner.free(p_object);
} else {
ERR_PRINT("Attempted to free a NavMeshGenerator2D RID that did not exist (or was already freed).");
}
}
void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data) {
if (p_navigation_mesh.is_null() || p_source_geometry_data.is_null()) {
return;

23
modules/navigation/2d/nav_mesh_generator_2d.h
View file

@ -36,6 +36,7 @@
#include "core/object/class_db.h"
#include "core/object/worker_thread_pool.h"
#include "core/templates/rid_owner.h"
#include "servers/navigation_server_2d.h"
class Node;
class NavigationPolygon;
@ -47,12 +48,7 @@ class NavMeshGenerator2D : public Object {
static Mutex baking_navmesh_mutex;
static Mutex generator_task_mutex;
static RWLock generator_rid_rwlock;
struct NavMeshGeometryParser2D {
RID self;
Callable callback;
};
static RID_Owner<NavMeshGeometryParser2D> generator_parser_owner;
static RWLock generator_parsers_rwlock;
static LocalVector<NavMeshGeometryParser2D *> generator_parsers;
static bool use_threads;
@ -85,13 +81,6 @@ class NavMeshGenerator2D : public Object {
static void generator_parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node);
static void generator_bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data);
static void generator_parse_meshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static void generator_parse_multimeshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static void generator_parse_polygon2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static void generator_parse_staticbody2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static void generator_parse_tile_map_layer_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static void generator_parse_navigationobstacle_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
static bool generator_emit_callback(const Callable &p_callback);
public:
@ -101,17 +90,13 @@ public:
static void cleanup();
static void finish();
static void set_generator_parsers(LocalVector<NavMeshGeometryParser2D *> p_parsers);
static void parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
static void bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback = Callable());
static void bake_from_source_geometry_data_async(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback = Callable());
static bool is_baking(Ref<NavigationPolygon> p_navigation_polygon);
static RID source_geometry_parser_create();
static void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback);
static bool owns(RID p_object);
static void free(RID p_object);
NavMeshGenerator2D();
~NavMeshGenerator2D();
};

42
modules/navigation/3d/godot_navigation_server_3d.cpp
View file

@ -1270,10 +1270,17 @@ COMMAND_1(free, RID, p_object) {
} else if (obstacle_owner.owns(p_object)) {
internal_free_obstacle(p_object);
} else if (geometry_parser_owner.owns(p_object)) {
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(p_object);
ERR_FAIL_NULL(parser);
generator_parsers.erase(parser);
#ifndef _3D_DISABLED
} else if (navmesh_generator_3d && navmesh_generator_3d->owns(p_object)) {
navmesh_generator_3d->free(p_object);
#endif // _3D_DISABLED
NavMeshGenerator3D::get_singleton()->set_generator_parsers(generator_parsers);
#endif
geometry_parser_owner.free(parser->self);
} else {
ERR_PRINT("Attempted to free a NavigationServer RID that did not exist (or was already freed).");
@ -1408,6 +1415,8 @@ void GodotNavigationServer3D::process(real_t p_delta_time) {
void GodotNavigationServer3D::init() {
#ifndef _3D_DISABLED
navmesh_generator_3d = memnew(NavMeshGenerator3D);
RWLockRead read_lock(geometry_parser_rwlock);
navmesh_generator_3d->set_generator_parsers(generator_parsers);
#endif // _3D_DISABLED
}
@ -1433,20 +1442,27 @@ void GodotNavigationServer3D::query_path(const Ref<NavigationPathQueryParameters
}
RID GodotNavigationServer3D::source_geometry_parser_create() {
RWLockWrite write_lock(geometry_parser_rwlock);
RID rid = geometry_parser_owner.make_rid();
NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(rid);
parser->self = rid;
generator_parsers.push_back(parser);
#ifndef _3D_DISABLED
if (navmesh_generator_3d) {
return navmesh_generator_3d->source_geometry_parser_create();
}
#endif // _3D_DISABLED
return RID();
NavMeshGenerator3D::get_singleton()->set_generator_parsers(generator_parsers);
#endif
return rid;
}
void GodotNavigationServer3D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
#ifndef _3D_DISABLED
if (navmesh_generator_3d) {
navmesh_generator_3d->source_geometry_parser_set_callback(p_parser, p_callback);
}
#endif // _3D_DISABLED
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(p_parser);
ERR_FAIL_NULL(parser);
parser->callback = p_callback;
}
Vector<Vector3> GodotNavigationServer3D::simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) {

465
modules/navigation/3d/nav_mesh_generator_3d.cpp
View file

@ -33,46 +33,23 @@
#include "nav_mesh_generator_3d.h"
#include "core/config/project_settings.h"
#include "core/math/convex_hull.h"
#include "core/os/thread.h"
#include "scene/3d/mesh_instance_3d.h"
#include "scene/3d/multimesh_instance_3d.h"
#include "scene/3d/navigation_obstacle_3d.h"
#include "scene/3d/physics/static_body_3d.h"
#include "scene/resources/3d/box_shape_3d.h"
#include "scene/resources/3d/capsule_shape_3d.h"
#include "scene/resources/3d/concave_polygon_shape_3d.h"
#include "scene/resources/3d/convex_polygon_shape_3d.h"
#include "scene/resources/3d/cylinder_shape_3d.h"
#include "scene/resources/3d/height_map_shape_3d.h"
#include "scene/3d/node_3d.h"
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
#include "scene/resources/3d/primitive_meshes.h"
#include "scene/resources/3d/shape_3d.h"
#include "scene/resources/3d/sphere_shape_3d.h"
#include "scene/resources/navigation_mesh.h"
#include "modules/modules_enabled.gen.h" // For csg, gridmap.
#ifdef MODULE_CSG_ENABLED
#include "modules/csg/csg_shape.h"
#endif
#ifdef MODULE_GRIDMAP_ENABLED
#include "modules/gridmap/grid_map.h"
#endif
#include <Recast.h>
NavMeshGenerator3D *NavMeshGenerator3D::singleton = nullptr;
Mutex NavMeshGenerator3D::baking_navmesh_mutex;
Mutex NavMeshGenerator3D::generator_task_mutex;
RWLock NavMeshGenerator3D::generator_rid_rwlock;
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;
HashSet<Ref<NavigationMesh>> NavMeshGenerator3D::baking_navmeshes;
HashMap<WorkerThreadPool::TaskID, NavMeshGenerator3D::NavMeshGeneratorTask3D *> NavMeshGenerator3D::generator_tasks;
RID_Owner<NavMeshGenerator3D::NavMeshGeometryParser3D> NavMeshGenerator3D::generator_parser_owner;
LocalVector<NavMeshGenerator3D::NavMeshGeometryParser3D *> NavMeshGenerator3D::generator_parsers;
LocalVector<NavMeshGeometryParser3D *> NavMeshGenerator3D::generator_parsers;
NavMeshGenerator3D *NavMeshGenerator3D::get_singleton() {
return singleton;
@ -141,12 +118,9 @@ void NavMeshGenerator3D::cleanup() {
}
generator_tasks.clear();
generator_rid_rwlock.write_lock();
for (NavMeshGeometryParser3D *parser : generator_parsers) {
generator_parser_owner.free(parser->self);
}
generator_parsers_rwlock.write_lock();
generator_parsers.clear();
generator_rid_rwlock.write_unlock();
generator_parsers_rwlock.write_unlock();
}
}
@ -247,25 +221,14 @@ void NavMeshGenerator3D::generator_thread_bake(void *p_arg) {
}
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_parse_meshinstance3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_parse_multimeshinstance3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_parse_staticbody3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
#ifdef MODULE_CSG_ENABLED
generator_parse_csgshape3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
#endif
#ifdef MODULE_GRIDMAP_ENABLED
generator_parse_gridmap_node(p_navigation_mesh, p_source_geometry_data, p_node);
#endif
generator_parse_navigationobstacle_node(p_navigation_mesh, p_source_geometry_data, p_node);
generator_rid_rwlock.read_lock();
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_rid_rwlock.read_unlock();
generator_parsers_rwlock.read_unlock();
if (p_recurse_children) {
for (int i = 0; i < p_node->get_child_count(); i++) {
@ -274,376 +237,9 @@ void NavMeshGenerator3D::generator_parse_geometry_node(const Ref<NavigationMesh>
}
}
void NavMeshGenerator3D::generator_parse_meshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
MeshInstance3D *mesh_instance = Object::cast_to<MeshInstance3D>(p_node);
if (mesh_instance) {
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
Ref<Mesh> mesh = mesh_instance->get_mesh();
if (mesh.is_valid()) {
p_source_geometry_data->add_mesh(mesh, mesh_instance->get_global_transform());
}
}
}
}
void NavMeshGenerator3D::generator_parse_multimeshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
MultiMeshInstance3D *multimesh_instance = Object::cast_to<MultiMeshInstance3D>(p_node);
if (multimesh_instance) {
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
Ref<MultiMesh> multimesh = multimesh_instance->get_multimesh();
if (multimesh.is_valid()) {
Ref<Mesh> mesh = multimesh->get_mesh();
if (mesh.is_valid()) {
int n = multimesh->get_visible_instance_count();
if (n == -1) {
n = multimesh->get_instance_count();
}
for (int i = 0; i < n; i++) {
p_source_geometry_data->add_mesh(mesh, multimesh_instance->get_global_transform() * multimesh->get_instance_transform(i));
}
}
}
}
}
}
void NavMeshGenerator3D::generator_parse_staticbody3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
StaticBody3D *static_body = Object::cast_to<StaticBody3D>(p_node);
if (static_body) {
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (static_body->get_collision_layer() & parsed_collision_mask)) {
List<uint32_t> shape_owners;
static_body->get_shape_owners(&shape_owners);
for (uint32_t shape_owner : shape_owners) {
if (static_body->is_shape_owner_disabled(shape_owner)) {
continue;
}
const int shape_count = static_body->shape_owner_get_shape_count(shape_owner);
for (int shape_index = 0; shape_index < shape_count; shape_index++) {
Ref<Shape3D> s = static_body->shape_owner_get_shape(shape_owner, shape_index);
if (s.is_null()) {
continue;
}
const Transform3D transform = static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner);
BoxShape3D *box = Object::cast_to<BoxShape3D>(*s);
if (box) {
Array arr;
arr.resize(RS::ARRAY_MAX);
BoxMesh::create_mesh_array(arr, box->get_size());
p_source_geometry_data->add_mesh_array(arr, transform);
}
CapsuleShape3D *capsule = Object::cast_to<CapsuleShape3D>(*s);
if (capsule) {
Array arr;
arr.resize(RS::ARRAY_MAX);
CapsuleMesh::create_mesh_array(arr, capsule->get_radius(), capsule->get_height());
p_source_geometry_data->add_mesh_array(arr, transform);
}
CylinderShape3D *cylinder = Object::cast_to<CylinderShape3D>(*s);
if (cylinder) {
Array arr;
arr.resize(RS::ARRAY_MAX);
CylinderMesh::create_mesh_array(arr, cylinder->get_radius(), cylinder->get_radius(), cylinder->get_height());
p_source_geometry_data->add_mesh_array(arr, transform);
}
SphereShape3D *sphere = Object::cast_to<SphereShape3D>(*s);
if (sphere) {
Array arr;
arr.resize(RS::ARRAY_MAX);
SphereMesh::create_mesh_array(arr, sphere->get_radius(), sphere->get_radius() * 2.0);
p_source_geometry_data->add_mesh_array(arr, transform);
}
ConcavePolygonShape3D *concave_polygon = Object::cast_to<ConcavePolygonShape3D>(*s);
if (concave_polygon) {
p_source_geometry_data->add_faces(concave_polygon->get_faces(), transform);
}
ConvexPolygonShape3D *convex_polygon = Object::cast_to<ConvexPolygonShape3D>(*s);
if (convex_polygon) {
Vector<Vector3> varr = Variant(convex_polygon->get_points());
Geometry3D::MeshData md;
Error err = ConvexHullComputer::convex_hull(varr, md);
if (err == OK) {
PackedVector3Array faces;
for (const Geometry3D::MeshData::Face &face : md.faces) {
for (uint32_t k = 2; k < face.indices.size(); ++k) {
faces.push_back(md.vertices[face.indices[0]]);
faces.push_back(md.vertices[face.indices[k - 1]]);
faces.push_back(md.vertices[face.indices[k]]);
}
}
p_source_geometry_data->add_faces(faces, transform);
}
}
HeightMapShape3D *heightmap_shape = Object::cast_to<HeightMapShape3D>(*s);
if (heightmap_shape) {
int heightmap_depth = heightmap_shape->get_map_depth();
int heightmap_width = heightmap_shape->get_map_width();
if (heightmap_depth >= 2 && heightmap_width >= 2) {
const Vector<real_t> &map_data = heightmap_shape->get_map_data();
Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1);
Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5;
Vector<Vector3> vertex_array;
vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6);
Vector3 *vertex_array_ptrw = vertex_array.ptrw();
const real_t *map_data_ptr = map_data.ptr();
int vertex_index = 0;
for (int d = 0; d < heightmap_depth - 1; d++) {
for (int w = 0; w < heightmap_width - 1; w++) {
vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d);
vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1);
vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_index += 6;
}
}
if (vertex_array.size() > 0) {
p_source_geometry_data->add_faces(vertex_array, transform);
}
}
}
}
}
}
}
}
#ifdef MODULE_CSG_ENABLED
void NavMeshGenerator3D::generator_parse_csgshape3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
CSGShape3D *csgshape3d = Object::cast_to<CSGShape3D>(p_node);
if (csgshape3d) {
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS && csgshape3d->is_using_collision() && (csgshape3d->get_collision_layer() & parsed_collision_mask)) || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
CSGShape3D *csg_shape = Object::cast_to<CSGShape3D>(p_node);
Array meshes = csg_shape->get_meshes();
if (!meshes.is_empty()) {
Ref<Mesh> mesh = meshes[1];
if (mesh.is_valid()) {
p_source_geometry_data->add_mesh(mesh, csg_shape->get_global_transform());
}
}
}
}
}
#endif // MODULE_CSG_ENABLED
#ifdef MODULE_GRIDMAP_ENABLED
void NavMeshGenerator3D::generator_parse_gridmap_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
GridMap *gridmap = Object::cast_to<GridMap>(p_node);
if (gridmap) {
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
Array meshes = gridmap->get_meshes();
Transform3D xform = gridmap->get_global_transform();
for (int i = 0; i < meshes.size(); i += 2) {
Ref<Mesh> mesh = meshes[i + 1];
if (mesh.is_valid()) {
p_source_geometry_data->add_mesh(mesh, xform * (Transform3D)meshes[i]);
}
}
}
else if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (gridmap->get_collision_layer() & parsed_collision_mask)) {
Array shapes = gridmap->get_collision_shapes();
for (int i = 0; i < shapes.size(); i += 2) {
RID shape = shapes[i + 1];
PhysicsServer3D::ShapeType type = PhysicsServer3D::get_singleton()->shape_get_type(shape);
Variant data = PhysicsServer3D::get_singleton()->shape_get_data(shape);
switch (type) {
case PhysicsServer3D::SHAPE_SPHERE: {
real_t radius = data;
Array arr;
arr.resize(RS::ARRAY_MAX);
SphereMesh::create_mesh_array(arr, radius, radius * 2.0);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_BOX: {
Vector3 extents = data;
Array arr;
arr.resize(RS::ARRAY_MAX);
BoxMesh::create_mesh_array(arr, extents * 2.0);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_CAPSULE: {
Dictionary dict = data;
real_t radius = dict["radius"];
real_t height = dict["height"];
Array arr;
arr.resize(RS::ARRAY_MAX);
CapsuleMesh::create_mesh_array(arr, radius, height);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_CYLINDER: {
Dictionary dict = data;
real_t radius = dict["radius"];
real_t height = dict["height"];
Array arr;
arr.resize(RS::ARRAY_MAX);
CylinderMesh::create_mesh_array(arr, radius, radius, height);
p_source_geometry_data->add_mesh_array(arr, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_CONVEX_POLYGON: {
PackedVector3Array vertices = data;
Geometry3D::MeshData md;
Error err = ConvexHullComputer::convex_hull(vertices, md);
if (err == OK) {
PackedVector3Array faces;
for (const Geometry3D::MeshData::Face &face : md.faces) {
for (uint32_t k = 2; k < face.indices.size(); ++k) {
faces.push_back(md.vertices[face.indices[0]]);
faces.push_back(md.vertices[face.indices[k - 1]]);
faces.push_back(md.vertices[face.indices[k]]);
}
}
p_source_geometry_data->add_faces(faces, shapes[i]);
}
} break;
case PhysicsServer3D::SHAPE_CONCAVE_POLYGON: {
Dictionary dict = data;
PackedVector3Array faces = Variant(dict["faces"]);
p_source_geometry_data->add_faces(faces, shapes[i]);
} break;
case PhysicsServer3D::SHAPE_HEIGHTMAP: {
Dictionary dict = data;
///< dict( int:"width", int:"depth",float:"cell_size", float_array:"heights"
int heightmap_depth = dict["depth"];
int heightmap_width = dict["width"];
if (heightmap_depth >= 2 && heightmap_width >= 2) {
const Vector<real_t> &map_data = dict["heights"];
Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1);
Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5;
Vector<Vector3> vertex_array;
vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6);
Vector3 *vertex_array_ptrw = vertex_array.ptrw();
const real_t *map_data_ptr = map_data.ptr();
int vertex_index = 0;
for (int d = 0; d < heightmap_depth - 1; d++) {
for (int w = 0; w < heightmap_width - 1; w++) {
vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d);
vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1);
vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_index += 6;
}
}
if (vertex_array.size() > 0) {
p_source_geometry_data->add_faces(vertex_array, shapes[i]);
}
}
} break;
default: {
WARN_PRINT("Unsupported collision shape type.");
} break;
}
}
}
}
}
#endif // MODULE_GRIDMAP_ENABLED
void NavMeshGenerator3D::generator_parse_navigationobstacle_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
NavigationObstacle3D *obstacle = Object::cast_to<NavigationObstacle3D>(p_node);
if (obstacle == nullptr) {
return;
}
if (!obstacle->get_affect_navigation_mesh()) {
return;
}
const float elevation = obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y;
// Prevent non-positive scaling.
const Vector3 safe_scale = obstacle->get_global_basis().get_scale().abs().maxf(0.001);
const float obstacle_radius = obstacle->get_radius();
if (obstacle_radius > 0.0) {
// Radius defined obstacle should be uniformly scaled from obstacle basis max scale axis.
const float scaling_max_value = safe_scale[safe_scale.max_axis_index()];
const Vector3 uniform_max_scale = Vector3(scaling_max_value, scaling_max_value, scaling_max_value);
const Transform3D obstacle_circle_transform = p_source_geometry_data->root_node_transform * Transform3D(Basis().scaled(uniform_max_scale), obstacle->get_global_position());
Vector<Vector3> obstruction_circle_vertices;
// The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding.
// Without, navigation paths can go directly through the middle of the obstacle and conflict with the avoidance to get agents stuck.
// No place for excessive "round" detail here. Every additional edge adds a high cost for something that needs to be quick, not pretty.
static const int circle_points = 12;
obstruction_circle_vertices.resize(circle_points);
Vector3 *circle_vertices_ptrw = obstruction_circle_vertices.ptrw();
const real_t circle_point_step = Math_TAU / circle_points;
for (int i = 0; i < circle_points; i++) {
const float angle = i * circle_point_step;
circle_vertices_ptrw[i] = obstacle_circle_transform.xform(Vector3(Math::cos(angle) * obstacle_radius, 0.0, Math::sin(angle) * obstacle_radius));
}
p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, elevation - obstacle_radius, scaling_max_value * obstacle_radius, obstacle->get_carve_navigation_mesh());
}
// Obstacles are projected to the xz-plane, so only rotation around the y-axis can be taken into account.
const Transform3D node_xform = p_source_geometry_data->root_node_transform * Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle->get_global_rotation().y), obstacle->get_global_position());
const Vector<Vector3> &obstacle_vertices = obstacle->get_vertices();
if (obstacle_vertices.is_empty()) {
return;
}
Vector<Vector3> obstruction_shape_vertices;
obstruction_shape_vertices.resize(obstacle_vertices.size());
const Vector3 *obstacle_vertices_ptr = obstacle_vertices.ptr();
Vector3 *obstruction_shape_vertices_ptrw = obstruction_shape_vertices.ptrw();
for (int i = 0; i < obstacle_vertices.size(); i++) {
obstruction_shape_vertices_ptrw[i] = node_xform.xform(obstacle_vertices_ptr[i]);
obstruction_shape_vertices_ptrw[i].y = 0.0;
}
p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, elevation, safe_scale.y * obstacle->get_height(), obstacle->get_carve_navigation_mesh());
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) {
@ -965,45 +561,4 @@ bool NavMeshGenerator3D::generator_emit_callback(const Callable &p_callback) {
return ce.error == Callable::CallError::CALL_OK;
}
RID NavMeshGenerator3D::source_geometry_parser_create() {
RWLockWrite write_lock(generator_rid_rwlock);
RID rid = generator_parser_owner.make_rid();
NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(rid);
parser->self = rid;
generator_parsers.push_back(parser);
return rid;
}
void NavMeshGenerator3D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
RWLockWrite write_lock(generator_rid_rwlock);
NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(p_parser);
ERR_FAIL_NULL(parser);
parser->callback = p_callback;
}
bool NavMeshGenerator3D::owns(RID p_object) {
RWLockRead read_lock(generator_rid_rwlock);
return generator_parser_owner.owns(p_object);
}
void NavMeshGenerator3D::free(RID p_object) {
RWLockWrite write_lock(generator_rid_rwlock);
if (generator_parser_owner.owns(p_object)) {
NavMeshGeometryParser3D *parser = generator_parser_owner.get_or_null(p_object);
generator_parsers.erase(parser);
generator_parser_owner.free(p_object);
} else {
ERR_PRINT("Attempted to free a NavMeshGenerator3D RID that did not exist (or was already freed).");
}
}
#endif // _3D_DISABLED

29
modules/navigation/3d/nav_mesh_generator_3d.h
View file

@ -36,8 +36,7 @@
#include "core/object/class_db.h"
#include "core/object/worker_thread_pool.h"
#include "core/templates/rid_owner.h"
#include "modules/modules_enabled.gen.h" // For csg, gridmap.
#include "servers/navigation_server_3d.h"
class Node;
class NavigationMesh;
@ -49,12 +48,7 @@ class NavMeshGenerator3D : public Object {
static Mutex baking_navmesh_mutex;
static Mutex generator_task_mutex;
static RWLock generator_rid_rwlock;
struct NavMeshGeometryParser3D {
RID self;
Callable callback;
};
static RID_Owner<NavMeshGeometryParser3D> generator_parser_owner;
static RWLock generator_parsers_rwlock;
static LocalVector<NavMeshGeometryParser3D *> generator_parsers;
static bool use_threads;
@ -87,17 +81,6 @@ class NavMeshGenerator3D : public Object {
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(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data);
static void generator_parse_meshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
static void generator_parse_multimeshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
static void generator_parse_staticbody3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
#ifdef MODULE_CSG_ENABLED
static void generator_parse_csgshape3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
#endif // MODULE_CSG_ENABLED
#ifdef MODULE_GRIDMAP_ENABLED
static void generator_parse_gridmap_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
#endif // MODULE_GRIDMAP_ENABLED
static void generator_parse_navigationobstacle_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
static bool generator_emit_callback(const Callable &p_callback);
public:
@ -107,17 +90,13 @@ public:
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 RID source_geometry_parser_create();
static void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback);
static bool owns(RID p_object);
static void free(RID p_object);
NavMeshGenerator3D();
~NavMeshGenerator3D();
};

106
scene/2d/mesh_instance_2d.cpp
View file

@ -30,6 +30,17 @@
#include "mesh_instance_2d.h"
#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h"
#include "scene/resources/2d/navigation_polygon.h"
#include "scene/scene_string_names.h"
#include "servers/navigation_server_2d.h"
#include "thirdparty/clipper2/include/clipper2/clipper.h"
#include "thirdparty/misc/polypartition.h"
Callable MeshInstance2D::_navmesh_source_geometry_parsing_callback;
RID MeshInstance2D::_navmesh_source_geometry_parser;
void MeshInstance2D::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_DRAW: {
@ -106,5 +117,100 @@ bool MeshInstance2D::_edit_use_rect() const {
}
#endif // DEBUG_ENABLED
void MeshInstance2D::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer2D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&MeshInstance2D::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create();
NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void MeshInstance2D::navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
MeshInstance2D *mesh_instance = Object::cast_to<MeshInstance2D>(p_node);
if (mesh_instance == nullptr) {
return;
}
NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) {
return;
}
Ref<Mesh> mesh = mesh_instance->get_mesh();
if (mesh.is_null()) {
return;
}
const Transform2D mesh_instance_xform = p_source_geometry_data->root_node_transform * mesh_instance->get_global_transform();
using namespace Clipper2Lib;
PathsD subject_paths, dummy_clip_paths;
for (int i = 0; i < mesh->get_surface_count(); i++) {
if (mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) {
continue;
}
if (!(mesh->surface_get_format(i) & Mesh::ARRAY_FLAG_USE_2D_VERTICES)) {
continue;
}
PathD subject_path;
int index_count = 0;
if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
index_count = mesh->surface_get_array_index_len(i);
} else {
index_count = mesh->surface_get_array_len(i);
}
ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0));
Array a = mesh->surface_get_arrays(i);
Vector<Vector2> mesh_vertices = a[Mesh::ARRAY_VERTEX];
if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
Vector<int> mesh_indices = a[Mesh::ARRAY_INDEX];
for (int vertex_index : mesh_indices) {
const Vector2 &vertex = mesh_vertices[vertex_index];
const PointD &point = PointD(vertex.x, vertex.y);
subject_path.push_back(point);
}
} else {
for (const Vector2 &vertex : mesh_vertices) {
const PointD &point = PointD(vertex.x, vertex.y);
subject_path.push_back(point);
}
}
subject_paths.push_back(subject_path);
}
PathsD path_solution;
path_solution = Union(subject_paths, dummy_clip_paths, FillRule::NonZero);
//path_solution = RamerDouglasPeucker(path_solution, 0.025);
Vector<Vector<Vector2>> polypaths;
for (const PathD &scaled_path : path_solution) {
Vector<Vector2> shape_outline;
for (const PointD &scaled_point : scaled_path) {
shape_outline.push_back(Point2(static_cast<real_t>(scaled_point.x), static_cast<real_t>(scaled_point.y)));
}
for (int i = 0; i < shape_outline.size(); i++) {
shape_outline.write[i] = mesh_instance_xform.xform(shape_outline[i]);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
}
MeshInstance2D::MeshInstance2D() {
}

11
scene/2d/mesh_instance_2d.h
View file

@ -33,6 +33,9 @@
#include "scene/2d/node_2d.h"
class NavigationPolygon;
class NavigationMeshSourceGeometryData2D;
class MeshInstance2D : public Node2D {
GDCLASS(MeshInstance2D, Node2D);
@ -56,6 +59,14 @@ public:
void set_texture(const Ref<Texture2D> &p_texture);
Ref<Texture2D> get_texture() const;
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
MeshInstance2D();
};

115
scene/2d/multimesh_instance_2d.cpp
View file

@ -30,6 +30,17 @@
#include "multimesh_instance_2d.h"
#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h"
#include "scene/resources/2d/navigation_polygon.h"
#include "scene/scene_string_names.h"
#include "servers/navigation_server_2d.h"
#include "thirdparty/clipper2/include/clipper2/clipper.h"
#include "thirdparty/misc/polypartition.h"
Callable MultiMeshInstance2D::_navmesh_source_geometry_parsing_callback;
RID MultiMeshInstance2D::_navmesh_source_geometry_parser;
void MultiMeshInstance2D::_notification(int p_what) {
switch (p_what) {
case NOTIFICATION_DRAW: {
@ -95,6 +106,110 @@ Rect2 MultiMeshInstance2D::_edit_get_rect() const {
}
#endif // DEBUG_ENABLED
void MultiMeshInstance2D::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer2D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&MultiMeshInstance2D::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create();
NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void MultiMeshInstance2D::navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
MultiMeshInstance2D *multimesh_instance = Object::cast_to<MultiMeshInstance2D>(p_node);
if (multimesh_instance == nullptr) {
return;
}
NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) {
return;
}
Ref<MultiMesh> multimesh = multimesh_instance->get_multimesh();
if (!(multimesh.is_valid() && multimesh->get_transform_format() == MultiMesh::TRANSFORM_2D)) {
return;
}
Ref<Mesh> mesh = multimesh->get_mesh();
if (mesh.is_null()) {
return;
}
using namespace Clipper2Lib;
PathsD mesh_subject_paths, dummy_clip_paths;
for (int i = 0; i < mesh->get_surface_count(); i++) {
if (mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) {
continue;
}
if (!(mesh->surface_get_format(i) & Mesh::ARRAY_FLAG_USE_2D_VERTICES)) {
continue;
}
PathD subject_path;
int index_count = 0;
if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
index_count = mesh->surface_get_array_index_len(i);
} else {
index_count = mesh->surface_get_array_len(i);
}
ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0));
Array a = mesh->surface_get_arrays(i);
Vector<Vector2> mesh_vertices = a[Mesh::ARRAY_VERTEX];
if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
Vector<int> mesh_indices = a[Mesh::ARRAY_INDEX];
for (int vertex_index : mesh_indices) {
const Vector2 &vertex = mesh_vertices[vertex_index];
const PointD &point = PointD(vertex.x, vertex.y);
subject_path.push_back(point);
}
} else {
for (const Vector2 &vertex : mesh_vertices) {
const PointD &point = PointD(vertex.x, vertex.y);
subject_path.push_back(point);
}
}
mesh_subject_paths.push_back(subject_path);
}
PathsD mesh_path_solution = Union(mesh_subject_paths, dummy_clip_paths, FillRule::NonZero);
//path_solution = RamerDouglasPeucker(path_solution, 0.025);
int multimesh_instance_count = multimesh->get_visible_instance_count();
if (multimesh_instance_count == -1) {
multimesh_instance_count = multimesh->get_instance_count();
}
const Transform2D multimesh_instance_xform = p_source_geometry_data->root_node_transform * multimesh_instance->get_global_transform();
for (int i = 0; i < multimesh_instance_count; i++) {
const Transform2D multimesh_instance_mesh_instance_xform = multimesh_instance_xform * multimesh->get_instance_transform_2d(i);
for (const PathD &mesh_path : mesh_path_solution) {
Vector<Vector2> shape_outline;
for (const PointD &mesh_path_point : mesh_path) {
shape_outline.push_back(Point2(static_cast<real_t>(mesh_path_point.x), static_cast<real_t>(mesh_path_point.y)));
}
for (int j = 0; j < shape_outline.size(); j++) {
shape_outline.write[j] = multimesh_instance_mesh_instance_xform.xform(shape_outline[j]);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
}
}
MultiMeshInstance2D::MultiMeshInstance2D() {
}

11
scene/2d/multimesh_instance_2d.h
View file

@ -34,6 +34,9 @@
#include "scene/2d/node_2d.h"
#include "scene/resources/multimesh.h"
class NavigationPolygon;
class NavigationMeshSourceGeometryData2D;
class MultiMeshInstance2D : public Node2D {
GDCLASS(MultiMeshInstance2D, Node2D);
@ -56,6 +59,14 @@ public:
void set_texture(const Ref<Texture2D> &p_texture);
Ref<Texture2D> get_texture() const;
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
MultiMeshInstance2D();
~MultiMeshInstance2D();
};

74
scene/2d/navigation_obstacle_2d.cpp
View file

@ -31,9 +31,14 @@
#include "navigation_obstacle_2d.h"
#include "core/math/geometry_2d.h"
#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h"
#include "scene/resources/2d/navigation_polygon.h"
#include "scene/resources/world_2d.h"
#include "servers/navigation_server_2d.h"
Callable NavigationObstacle2D::_navmesh_source_geometry_parsing_callback;
RID NavigationObstacle2D::_navmesh_source_geometry_parser;
void NavigationObstacle2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_rid"), &NavigationObstacle2D::get_rid);
@ -339,6 +344,75 @@ PackedStringArray NavigationObstacle2D::get_configuration_warnings() const {
return warnings;
}
void NavigationObstacle2D::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer2D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&NavigationObstacle2D::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create();
NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void NavigationObstacle2D::navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
NavigationObstacle2D *obstacle = Object::cast_to<NavigationObstacle2D>(p_node);
if (obstacle == nullptr) {
return;
}
if (!obstacle->get_affect_navigation_mesh()) {
return;
}
const Vector2 safe_scale = obstacle->get_global_scale().abs().maxf(0.001);
const float obstacle_radius = obstacle->get_radius();
if (obstacle_radius > 0.0) {
// Radius defined obstacle should be uniformly scaled from obstacle basis max scale axis.
const float scaling_max_value = safe_scale[safe_scale.max_axis_index()];
const Vector2 uniform_max_scale = Vector2(scaling_max_value, scaling_max_value);
const Transform2D obstacle_circle_transform = p_source_geometry_data->root_node_transform * Transform2D(obstacle->get_global_rotation(), uniform_max_scale, 0.0, obstacle->get_global_position());
Vector<Vector2> obstruction_circle_vertices;
// The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding.
// Without, navigation paths can go directly through the middle of the obstacle and conflict with the avoidance to get agents stuck.
// No place for excessive "round" detail here. Every additional edge adds a high cost for something that needs to be quick, not pretty.
static const int circle_points = 12;
obstruction_circle_vertices.resize(circle_points);
Vector2 *circle_vertices_ptrw = obstruction_circle_vertices.ptrw();
const real_t circle_point_step = Math_TAU / circle_points;
for (int i = 0; i < circle_points; i++) {
const float angle = i * circle_point_step;
circle_vertices_ptrw[i] = obstacle_circle_transform.xform(Vector2(Math::cos(angle) * obstacle_radius, Math::sin(angle) * obstacle_radius));
}
p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, obstacle->get_carve_navigation_mesh());
}
// Obstacles are projected to the xz-plane, so only rotation around the y-axis can be taken into account.
const Transform2D node_xform = p_source_geometry_data->root_node_transform * obstacle->get_global_transform();
const Vector<Vector2> &obstacle_vertices = obstacle->get_vertices();
if (obstacle_vertices.is_empty()) {
return;
}
Vector<Vector2> obstruction_shape_vertices;
obstruction_shape_vertices.resize(obstacle_vertices.size());
const Vector2 *obstacle_vertices_ptr = obstacle_vertices.ptr();
Vector2 *obstruction_shape_vertices_ptrw = obstruction_shape_vertices.ptrw();
for (int i = 0; i < obstacle_vertices.size(); i++) {
obstruction_shape_vertices_ptrw[i] = node_xform.xform(obstacle_vertices_ptr[i]);
}
p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, obstacle->get_carve_navigation_mesh());
}
void NavigationObstacle2D::_update_map(RID p_map) {
map_current = p_map;
NavigationServer2D::get_singleton()->obstacle_set_map(obstacle, p_map);

11
scene/2d/navigation_obstacle_2d.h
View file

@ -33,6 +33,9 @@
#include "scene/2d/node_2d.h"
class NavigationPolygon;
class NavigationMeshSourceGeometryData2D;
class NavigationObstacle2D : public Node2D {
GDCLASS(NavigationObstacle2D, Node2D);
@ -115,6 +118,14 @@ public:
PackedStringArray get_configuration_warnings() const override;
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
private:
void _update_map(RID p_map);
void _update_position(const Vector2 p_position);

137
scene/2d/physics/static_body_2d.cpp
View file

@ -30,6 +30,18 @@
#include "static_body_2d.h"
#include "scene/resources/2d/capsule_shape_2d.h"
#include "scene/resources/2d/circle_shape_2d.h"
#include "scene/resources/2d/concave_polygon_shape_2d.h"
#include "scene/resources/2d/convex_polygon_shape_2d.h"
#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h"
#include "scene/resources/2d/navigation_polygon.h"
#include "scene/resources/2d/rectangle_shape_2d.h"
#include "servers/navigation_server_2d.h"
Callable StaticBody2D::_navmesh_source_geometry_parsing_callback;
RID StaticBody2D::_navmesh_source_geometry_parser;
void StaticBody2D::set_constant_linear_velocity(const Vector2 &p_vel) {
constant_linear_velocity = p_vel;
@ -77,6 +89,131 @@ void StaticBody2D::_reload_physics_characteristics() {
}
}
void StaticBody2D::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer2D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&StaticBody2D::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create();
NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void StaticBody2D::navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
StaticBody2D *static_body = Object::cast_to<StaticBody2D>(p_node);
if (static_body == nullptr) {
return;
}
NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) {
return;
}
uint32_t parsed_collision_mask = p_navigation_mesh->get_parsed_collision_mask();
if (!(static_body->get_collision_layer() & parsed_collision_mask)) {
return;
}
List<uint32_t> shape_owners;
static_body->get_shape_owners(&shape_owners);
for (uint32_t shape_owner : shape_owners) {
if (static_body->is_shape_owner_disabled(shape_owner)) {
continue;
}
const int shape_count = static_body->shape_owner_get_shape_count(shape_owner);
for (int shape_index = 0; shape_index < shape_count; shape_index++) {
Ref<Shape2D> s = static_body->shape_owner_get_shape(shape_owner, shape_index);
if (s.is_null()) {
continue;
}
const Transform2D static_body_xform = p_source_geometry_data->root_node_transform * static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner);
RectangleShape2D *rectangle_shape = Object::cast_to<RectangleShape2D>(*s);
if (rectangle_shape) {
Vector<Vector2> shape_outline;
const Vector2 &rectangle_size = rectangle_shape->get_size();
shape_outline.resize(5);
shape_outline.write[0] = static_body_xform.xform(-rectangle_size * 0.5);
shape_outline.write[1] = static_body_xform.xform(Vector2(rectangle_size.x, -rectangle_size.y) * 0.5);
shape_outline.write[2] = static_body_xform.xform(rectangle_size * 0.5);
shape_outline.write[3] = static_body_xform.xform(Vector2(-rectangle_size.x, rectangle_size.y) * 0.5);
shape_outline.write[4] = static_body_xform.xform(-rectangle_size * 0.5);
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
CapsuleShape2D *capsule_shape = Object::cast_to<CapsuleShape2D>(*s);
if (capsule_shape) {
const real_t capsule_height = capsule_shape->get_height();
const real_t capsule_radius = capsule_shape->get_radius();
Vector<Vector2> shape_outline;
const real_t turn_step = Math_TAU / 12.0;
shape_outline.resize(14);
int shape_outline_inx = 0;
for (int i = 0; i < 12; i++) {
Vector2 ofs = Vector2(0, (i > 3 && i <= 9) ? -capsule_height * 0.5 + capsule_radius : capsule_height * 0.5 - capsule_radius);
shape_outline.write[shape_outline_inx] = static_body_xform.xform(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * capsule_radius + ofs);
shape_outline_inx += 1;
if (i == 3 || i == 9) {
shape_outline.write[shape_outline_inx] = static_body_xform.xform(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * capsule_radius - ofs);
shape_outline_inx += 1;
}
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
CircleShape2D *circle_shape = Object::cast_to<CircleShape2D>(*s);
if (circle_shape) {
const real_t circle_radius = circle_shape->get_radius();
Vector<Vector2> shape_outline;
int circle_edge_count = 12;
shape_outline.resize(circle_edge_count);
const real_t turn_step = Math_TAU / real_t(circle_edge_count);
for (int i = 0; i < circle_edge_count; i++) {
shape_outline.write[i] = static_body_xform.xform(Vector2(Math::cos(i * turn_step), Math::sin(i * turn_step)) * circle_radius);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
ConcavePolygonShape2D *concave_polygon_shape = Object::cast_to<ConcavePolygonShape2D>(*s);
if (concave_polygon_shape) {
Vector<Vector2> shape_outline = concave_polygon_shape->get_segments();
for (int i = 0; i < shape_outline.size(); i++) {
shape_outline.write[i] = static_body_xform.xform(shape_outline[i]);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
ConvexPolygonShape2D *convex_polygon_shape = Object::cast_to<ConvexPolygonShape2D>(*s);
if (convex_polygon_shape) {
Vector<Vector2> shape_outline = convex_polygon_shape->get_points();
for (int i = 0; i < shape_outline.size(); i++) {
shape_outline.write[i] = static_body_xform.xform(shape_outline[i]);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
}
}
}
void StaticBody2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_constant_linear_velocity", "vel"), &StaticBody2D::set_constant_linear_velocity);
ClassDB::bind_method(D_METHOD("set_constant_angular_velocity", "vel"), &StaticBody2D::set_constant_angular_velocity);

11
scene/2d/physics/static_body_2d.h
View file

@ -33,6 +33,9 @@
#include "scene/2d/physics/physics_body_2d.h"
class NavigationPolygon;
class NavigationMeshSourceGeometryData2D;
class StaticBody2D : public PhysicsBody2D {
GDCLASS(StaticBody2D, PhysicsBody2D);
@ -57,6 +60,14 @@ public:
StaticBody2D(PhysicsServer2D::BodyMode p_mode = PhysicsServer2D::BODY_MODE_STATIC);
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
private:
void _reload_physics_characteristics();
};

36
scene/2d/polygon_2d.cpp
View file

@ -31,8 +31,14 @@
#include "polygon_2d.h"
#include "core/math/geometry_2d.h"
#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h"
#include "scene/resources/2d/navigation_polygon.h"
#include "servers/navigation_server_2d.h"
#include "skeleton_2d.h"
Callable Polygon2D::_navmesh_source_geometry_parsing_callback;
RID Polygon2D::_navmesh_source_geometry_parser;
#ifdef TOOLS_ENABLED
Dictionary Polygon2D::_edit_get_state() const {
Dictionary state = Node2D::_edit_get_state();
@ -604,6 +610,36 @@ NodePath Polygon2D::get_skeleton() const {
return skeleton;
}
void Polygon2D::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer2D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&Polygon2D::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create();
NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void Polygon2D::navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
Polygon2D *polygon_2d = Object::cast_to<Polygon2D>(p_node);
if (polygon_2d == nullptr) {
return;
}
NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH) {
const Transform2D polygon_2d_xform = p_source_geometry_data->root_node_transform * polygon_2d->get_global_transform();
Vector<Vector2> shape_outline = polygon_2d->get_polygon();
for (int i = 0; i < shape_outline.size(); i++) {
shape_outline.write[i] = polygon_2d_xform.xform(shape_outline[i]);
}
p_source_geometry_data->add_obstruction_outline(shape_outline);
}
}
void Polygon2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_polygon", "polygon"), &Polygon2D::set_polygon);
ClassDB::bind_method(D_METHOD("get_polygon"), &Polygon2D::get_polygon);

11
scene/2d/polygon_2d.h
View file

@ -33,6 +33,9 @@
#include "scene/2d/node_2d.h"
class NavigationPolygon;
class NavigationMeshSourceGeometryData2D;
class Polygon2D : public Node2D {
GDCLASS(Polygon2D, Node2D);
@ -150,6 +153,14 @@ public:
void set_skeleton(const NodePath &p_skeleton);
NodePath get_skeleton() const;
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
Polygon2D();
~Polygon2D();
};

33
scene/2d/tile_map.cpp
View file

@ -32,6 +32,8 @@
#include "tile_map.compat.inc"
#include "core/io/marshalls.h"
#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h"
#include "servers/navigation_server_2d.h"
#define TILEMAP_CALL_FOR_LAYER(layer, function, ...) \
if (layer < 0) { \
@ -47,6 +49,9 @@
ERR_FAIL_INDEX_V(layer, (int)layers.size(), err_value); \
return layers[layer]->function(__VA_ARGS__);
Callable TileMap::_navmesh_source_geometry_parsing_callback;
RID TileMap::_navmesh_source_geometry_parser;
void TileMap::_tile_set_changed() {
update_configuration_warnings();
}
@ -1022,5 +1027,33 @@ TileMap::TileMap() {
property_helper.setup_for_instance(base_property_helper, this);
}
void TileMap::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer2D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&TileMap::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create();
NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void TileMap::navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
TileMap *nb_tilemap = Object::cast_to<TileMap>(p_node);
if (nb_tilemap == nullptr) {
return;
}
// Special case for TileMap, so that internal layer get parsed even if p_recurse_children is false.
bool recurse_children = p_navigation_mesh->get_source_geometry_mode() != NavigationPolygon::SOURCE_GEOMETRY_GROUPS_EXPLICIT;
if (!recurse_children) {
for (int i = 0; i < p_node->get_child_count(); i++) {
TileMapLayer *tile_map_layer = Object::cast_to<TileMapLayer>(p_node->get_child(i));
if (tile_map_layer && tile_map_layer->get_index_in_tile_map() >= 0) {
tile_map_layer->navmesh_parse_source_geometry(p_navigation_mesh, p_source_geometry_data, tile_map_layer);
}
}
}
}
#undef TILEMAP_CALL_FOR_LAYER
#undef TILEMAP_CALL_FOR_LAYER_V

9
scene/2d/tile_map.h
View file

@ -36,6 +36,7 @@
#include "scene/resources/2d/tile_set.h"
class Control;
class NavigationMeshSourceGeometryData2D;
class TileMapLayer;
class TerrainConstraint;
@ -238,6 +239,14 @@ public:
// Configuration warnings.
PackedStringArray get_configuration_warnings() const override;
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
TileMap();
};

112
scene/2d/tile_map_layer.cpp
View file

@ -33,9 +33,13 @@
#include "core/io/marshalls.h"
#include "scene/2d/tile_map.h"
#include "scene/gui/control.h"
#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h"
#include "scene/resources/world_2d.h"
#include "servers/navigation_server_2d.h"
Callable TileMapLayer::_navmesh_source_geometry_parsing_callback;
RID TileMapLayer::_navmesh_source_geometry_parser;
#ifdef DEBUG_ENABLED
/////////////////////////////// Debug //////////////////////////////////////////
constexpr int TILE_MAP_DEBUG_QUADRANT_SIZE = 16;
@ -3065,6 +3069,114 @@ TileMapLayer::DebugVisibilityMode TileMapLayer::get_navigation_visibility_mode()
return navigation_visibility_mode;
}
void TileMapLayer::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer2D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&TileMapLayer::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer2D::get_singleton()->source_geometry_parser_create();
NavigationServer2D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void TileMapLayer::navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
TileMapLayer *tile_map_layer = Object::cast_to<TileMapLayer>(p_node);
if (tile_map_layer == nullptr) {
return;
}
Ref<TileSet> tile_set = tile_map_layer->get_tile_set();
if (tile_set.is_null()) {
return;
}
int physics_layers_count = tile_set->get_physics_layers_count();
int navigation_layers_count = tile_set->get_navigation_layers_count();
if (physics_layers_count <= 0 && navigation_layers_count <= 0) {
return;
}
NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
uint32_t parsed_collision_mask = p_navigation_mesh->get_parsed_collision_mask();
const Transform2D tilemap_xform = p_source_geometry_data->root_node_transform * tile_map_layer->get_global_transform();
TypedArray<Vector2i> used_cells = tile_map_layer->get_used_cells();
for (int used_cell_index = 0; used_cell_index < used_cells.size(); used_cell_index++) {
const Vector2i &cell = used_cells[used_cell_index];
const TileData *tile_data = tile_map_layer->get_cell_tile_data(cell);
if (tile_data == nullptr) {
continue;
}
// Transform flags.
const int alternative_id = tile_map_layer->get_cell_alternative_tile(cell);
bool flip_h = (alternative_id & TileSetAtlasSource::TRANSFORM_FLIP_H);
bool flip_v = (alternative_id & TileSetAtlasSource::TRANSFORM_FLIP_V);
bool transpose = (alternative_id & TileSetAtlasSource::TRANSFORM_TRANSPOSE);
Transform2D tile_transform;
tile_transform.set_origin(tile_map_layer->map_to_local(cell));
const Transform2D tile_transform_offset = tilemap_xform * tile_transform;
// Parse traversable polygons.
for (int navigation_layer = 0; navigation_layer < navigation_layers_count; navigation_layer++) {
Ref<NavigationPolygon> navigation_polygon = tile_data->get_navigation_polygon(navigation_layer, flip_h, flip_v, transpose);
if (navigation_polygon.is_valid()) {
for (int outline_index = 0; outline_index < navigation_polygon->get_outline_count(); outline_index++) {
const Vector<Vector2> &navigation_polygon_outline = navigation_polygon->get_outline(outline_index);
if (navigation_polygon_outline.is_empty()) {
continue;
}
Vector<Vector2> traversable_outline;
traversable_outline.resize(navigation_polygon_outline.size());
const Vector2 *navigation_polygon_outline_ptr = navigation_polygon_outline.ptr();
Vector2 *traversable_outline_ptrw = traversable_outline.ptrw();
for (int traversable_outline_index = 0; traversable_outline_index < traversable_outline.size(); traversable_outline_index++) {
traversable_outline_ptrw[traversable_outline_index] = tile_transform_offset.xform(navigation_polygon_outline_ptr[traversable_outline_index]);
}
p_source_geometry_data->_add_traversable_outline(traversable_outline);
}
}
}
// Parse obstacles.
for (int physics_layer = 0; physics_layer < physics_layers_count; physics_layer++) {
if ((parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH) &&
(tile_set->get_physics_layer_collision_layer(physics_layer) & parsed_collision_mask)) {
for (int collision_polygon_index = 0; collision_polygon_index < tile_data->get_collision_polygons_count(physics_layer); collision_polygon_index++) {
PackedVector2Array collision_polygon_points = tile_data->get_collision_polygon_points(physics_layer, collision_polygon_index);
if (collision_polygon_points.is_empty()) {
continue;
}
if (flip_h || flip_v || transpose) {
collision_polygon_points = TileData::get_transformed_vertices(collision_polygon_points, flip_h, flip_v, transpose);
}
Vector<Vector2> obstruction_outline;
obstruction_outline.resize(collision_polygon_points.size());
const Vector2 *collision_polygon_points_ptr = collision_polygon_points.ptr();
Vector2 *obstruction_outline_ptrw = obstruction_outline.ptrw();
for (int obstruction_outline_index = 0; obstruction_outline_index < obstruction_outline.size(); obstruction_outline_index++) {
obstruction_outline_ptrw[obstruction_outline_index] = tile_transform_offset.xform(collision_polygon_points_ptr[obstruction_outline_index]);
}
p_source_geometry_data->_add_obstruction_outline(obstruction_outline);
}
}
}
}
}
TileMapLayer::TileMapLayer() {
set_notify_transform(true);
}

9
scene/2d/tile_map_layer.h
View file

@ -33,6 +33,7 @@
#include "scene/resources/2d/tile_set.h"
class NavigationMeshSourceGeometryData2D;
class TileSetAtlasSource;
class TileMap;
@ -512,6 +513,14 @@ public:
void set_navigation_visibility_mode(DebugVisibilityMode p_show_navigation);
DebugVisibilityMode get_navigation_visibility_mode() const;
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
TileMapLayer();
~TileMapLayer();
};

33
scene/3d/mesh_instance_3d.cpp
View file

@ -36,6 +36,13 @@
#include "scene/resources/3d/concave_polygon_shape_3d.h"
#include "scene/resources/3d/convex_polygon_shape_3d.h"
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
#include "scene/resources/navigation_mesh.h"
#include "servers/navigation_server_3d.h"
Callable MeshInstance3D::_navmesh_source_geometry_parsing_callback;
RID MeshInstance3D::_navmesh_source_geometry_parser;
bool MeshInstance3D::_set(const StringName &p_name, const Variant &p_value) {
//this is not _too_ bad performance wise, really. it only arrives here if the property was not set anywhere else.
//add to it that it's probably found on first call to _set anyway.
@ -842,6 +849,32 @@ Ref<TriangleMesh> MeshInstance3D::generate_triangle_mesh() const {
return Ref<TriangleMesh>();
}
void MeshInstance3D::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer3D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&MeshInstance3D::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create();
NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void MeshInstance3D::navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
MeshInstance3D *mesh_instance = Object::cast_to<MeshInstance3D>(p_node);
if (mesh_instance == nullptr) {
return;
}
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
Ref<Mesh> mesh = mesh_instance->get_mesh();
if (mesh.is_valid()) {
p_source_geometry_data->add_mesh(mesh, mesh_instance->get_global_transform());
}
}
}
void MeshInstance3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_mesh", "mesh"), &MeshInstance3D::set_mesh);
ClassDB::bind_method(D_METHOD("get_mesh"), &MeshInstance3D::get_mesh);

11
scene/3d/mesh_instance_3d.h
View file

@ -33,6 +33,9 @@
#include "core/templates/local_vector.h"
#include "scene/3d/visual_instance_3d.h"
class NavigationMesh;
class NavigationMeshSourceGeometryData3D;
class Skin;
class SkinReference;
@ -106,6 +109,14 @@ public:
virtual Ref<TriangleMesh> generate_triangle_mesh() const override;
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
MeshInstance3D();
~MeshInstance3D();
};

42
scene/3d/multimesh_instance_3d.cpp
View file

@ -30,6 +30,13 @@
#include "multimesh_instance_3d.h"
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
#include "scene/resources/navigation_mesh.h"
#include "servers/navigation_server_3d.h"
Callable MultiMeshInstance3D::_navmesh_source_geometry_parsing_callback;
RID MultiMeshInstance3D::_navmesh_source_geometry_parser;
void MultiMeshInstance3D::_refresh_interpolated() {
if (is_inside_tree() && multimesh.is_valid()) {
bool interpolated = is_physics_interpolated_and_enabled();
@ -96,6 +103,41 @@ AABB MultiMeshInstance3D::get_aabb() const {
}
}
void MultiMeshInstance3D::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer3D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&MultiMeshInstance3D::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create();
NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void MultiMeshInstance3D::navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
MultiMeshInstance3D *multimesh_instance = Object::cast_to<MultiMeshInstance3D>(p_node);
if (multimesh_instance == nullptr) {
return;
}
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
Ref<MultiMesh> multimesh = multimesh_instance->get_multimesh();
if (multimesh.is_valid()) {
Ref<Mesh> mesh = multimesh->get_mesh();
if (mesh.is_valid()) {
int n = multimesh->get_visible_instance_count();
if (n == -1) {
n = multimesh->get_instance_count();
}
for (int i = 0; i < n; i++) {
p_source_geometry_data->add_mesh(mesh, multimesh_instance->get_global_transform() * multimesh->get_instance_transform(i));
}
}
}
}
}
MultiMeshInstance3D::MultiMeshInstance3D() {
}

11
scene/3d/multimesh_instance_3d.h
View file

@ -34,6 +34,9 @@
#include "scene/3d/visual_instance_3d.h"
#include "scene/resources/multimesh.h"
class NavigationMesh;
class NavigationMeshSourceGeometryData3D;
class MultiMeshInstance3D : public GeometryInstance3D {
GDCLASS(MultiMeshInstance3D, GeometryInstance3D);
@ -54,6 +57,14 @@ public:
virtual AABB get_aabb() const override;
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
MultiMeshInstance3D();
~MultiMeshInstance3D();
};

77
scene/3d/navigation_obstacle_3d.cpp
View file

@ -31,8 +31,13 @@
#include "navigation_obstacle_3d.h"
#include "core/math/geometry_2d.h"
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
#include "scene/resources/navigation_mesh.h"
#include "servers/navigation_server_3d.h"
Callable NavigationObstacle3D::_navmesh_source_geometry_parsing_callback;
RID NavigationObstacle3D::_navmesh_source_geometry_parser;
void NavigationObstacle3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_rid"), &NavigationObstacle3D::get_rid);
@ -419,6 +424,78 @@ PackedStringArray NavigationObstacle3D::get_configuration_warnings() const {
return warnings;
}
void NavigationObstacle3D::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer3D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&NavigationObstacle3D::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create();
NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void NavigationObstacle3D::navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
NavigationObstacle3D *obstacle = Object::cast_to<NavigationObstacle3D>(p_node);
if (obstacle == nullptr) {
return;
}
if (!obstacle->get_affect_navigation_mesh()) {
return;
}
const float elevation = obstacle->get_global_position().y + p_source_geometry_data->root_node_transform.origin.y;
// Prevent non-positive scaling.
const Vector3 safe_scale = obstacle->get_global_basis().get_scale().abs().maxf(0.001);
const float obstacle_radius = obstacle->get_radius();
if (obstacle_radius > 0.0) {
// Radius defined obstacle should be uniformly scaled from obstacle basis max scale axis.
const float scaling_max_value = safe_scale[safe_scale.max_axis_index()];
const Vector3 uniform_max_scale = Vector3(scaling_max_value, scaling_max_value, scaling_max_value);
const Transform3D obstacle_circle_transform = p_source_geometry_data->root_node_transform * Transform3D(Basis().scaled(uniform_max_scale), obstacle->get_global_position());
Vector<Vector3> obstruction_circle_vertices;
// The point of this is that the moving obstacle can make a simple hole in the navigation mesh and affect the pathfinding.
// Without, navigation paths can go directly through the middle of the obstacle and conflict with the avoidance to get agents stuck.
// No place for excessive "round" detail here. Every additional edge adds a high cost for something that needs to be quick, not pretty.
static const int circle_points = 12;
obstruction_circle_vertices.resize(circle_points);
Vector3 *circle_vertices_ptrw = obstruction_circle_vertices.ptrw();
const real_t circle_point_step = Math_TAU / circle_points;
for (int i = 0; i < circle_points; i++) {
const float angle = i * circle_point_step;
circle_vertices_ptrw[i] = obstacle_circle_transform.xform(Vector3(Math::cos(angle) * obstacle_radius, 0.0, Math::sin(angle) * obstacle_radius));
}
p_source_geometry_data->add_projected_obstruction(obstruction_circle_vertices, elevation - obstacle_radius, scaling_max_value * obstacle_radius, obstacle->get_carve_navigation_mesh());
}
// Obstacles are projected to the xz-plane, so only rotation around the y-axis can be taken into account.
const Transform3D node_xform = p_source_geometry_data->root_node_transform * Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle->get_global_rotation().y), obstacle->get_global_position());
const Vector<Vector3> &obstacle_vertices = obstacle->get_vertices();
if (obstacle_vertices.is_empty()) {
return;
}
Vector<Vector3> obstruction_shape_vertices;
obstruction_shape_vertices.resize(obstacle_vertices.size());
const Vector3 *obstacle_vertices_ptr = obstacle_vertices.ptr();
Vector3 *obstruction_shape_vertices_ptrw = obstruction_shape_vertices.ptrw();
for (int i = 0; i < obstacle_vertices.size(); i++) {
obstruction_shape_vertices_ptrw[i] = node_xform.xform(obstacle_vertices_ptr[i]);
obstruction_shape_vertices_ptrw[i].y = 0.0;
}
p_source_geometry_data->add_projected_obstruction(obstruction_shape_vertices, elevation, safe_scale.y * obstacle->get_height(), obstacle->get_carve_navigation_mesh());
}
void NavigationObstacle3D::_update_map(RID p_map) {
NavigationServer3D::get_singleton()->obstacle_set_map(obstacle, p_map);
map_current = p_map;

11
scene/3d/navigation_obstacle_3d.h
View file

@ -33,6 +33,9 @@
#include "scene/3d/node_3d.h"
class NavigationMesh;
class NavigationMeshSourceGeometryData3D;
class NavigationObstacle3D : public Node3D {
GDCLASS(NavigationObstacle3D, Node3D);
@ -123,6 +126,14 @@ public:
PackedStringArray get_configuration_warnings() const override;
private:
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
private:
void _update_map(RID p_map);
void _update_position(const Vector3 p_position);

150
scene/3d/physics/static_body_3d.cpp
View file

@ -30,6 +30,24 @@
#include "static_body_3d.h"
#include "core/math/convex_hull.h"
#include "scene/resources/3d/box_shape_3d.h"
#include "scene/resources/3d/capsule_shape_3d.h"
#include "scene/resources/3d/concave_polygon_shape_3d.h"
#include "scene/resources/3d/convex_polygon_shape_3d.h"
#include "scene/resources/3d/cylinder_shape_3d.h"
#include "scene/resources/3d/height_map_shape_3d.h"
#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
#include "scene/resources/3d/primitive_meshes.h"
#include "scene/resources/3d/shape_3d.h"
#include "scene/resources/3d/sphere_shape_3d.h"
#include "scene/resources/3d/world_boundary_shape_3d.h"
#include "scene/resources/navigation_mesh.h"
#include "servers/navigation_server_3d.h"
Callable StaticBody3D::_navmesh_source_geometry_parsing_callback;
RID StaticBody3D::_navmesh_source_geometry_parser;
void StaticBody3D::set_physics_material_override(const Ref<PhysicsMaterial> &p_physics_material_override) {
if (physics_material_override.is_valid()) {
physics_material_override->disconnect_changed(callable_mp(this, &StaticBody3D::_reload_physics_characteristics));
@ -77,6 +95,138 @@ void StaticBody3D::_reload_physics_characteristics() {
}
}
void StaticBody3D::navmesh_parse_init() {
ERR_FAIL_NULL(NavigationServer3D::get_singleton());
if (!_navmesh_source_geometry_parser.is_valid()) {
_navmesh_source_geometry_parsing_callback = callable_mp_static(&StaticBody3D::navmesh_parse_source_geometry);
_navmesh_source_geometry_parser = NavigationServer3D::get_singleton()->source_geometry_parser_create();
NavigationServer3D::get_singleton()->source_geometry_parser_set_callback(_navmesh_source_geometry_parser, _navmesh_source_geometry_parsing_callback);
}
}
void StaticBody3D::navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
StaticBody3D *static_body = Object::cast_to<StaticBody3D>(p_node);
if (static_body == nullptr) {
return;
}
NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (static_body->get_collision_layer() & parsed_collision_mask)) {
List<uint32_t> shape_owners;
static_body->get_shape_owners(&shape_owners);
for (uint32_t shape_owner : shape_owners) {
if (static_body->is_shape_owner_disabled(shape_owner)) {
continue;
}
const int shape_count = static_body->shape_owner_get_shape_count(shape_owner);
for (int shape_index = 0; shape_index < shape_count; shape_index++) {
Ref<Shape3D> s = static_body->shape_owner_get_shape(shape_owner, shape_index);
if (s.is_null()) {
continue;
}
const Transform3D transform = static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner);
BoxShape3D *box = Object::cast_to<BoxShape3D>(*s);
if (box) {
Array arr;
arr.resize(RS::ARRAY_MAX);
BoxMesh::create_mesh_array(arr, box->get_size());
p_source_geometry_data->add_mesh_array(arr, transform);
}
CapsuleShape3D *capsule = Object::cast_to<CapsuleShape3D>(*s);
if (capsule) {
Array arr;
arr.resize(RS::ARRAY_MAX);
CapsuleMesh::create_mesh_array(arr, capsule->get_radius(), capsule->get_height());
p_source_geometry_data->add_mesh_array(arr, transform);
}
CylinderShape3D *cylinder = Object::cast_to<CylinderShape3D>(*s);
if (cylinder) {
Array arr;
arr.resize(RS::ARRAY_MAX);
CylinderMesh::create_mesh_array(arr, cylinder->get_radius(), cylinder->get_radius(), cylinder->get_height());
p_source_geometry_data->add_mesh_array(arr, transform);
}
SphereShape3D *sphere = Object::cast_to<SphereShape3D>(*s);
if (sphere) {
Array arr;
arr.resize(RS::ARRAY_MAX);
SphereMesh::create_mesh_array(arr, sphere->get_radius(), sphere->get_radius() * 2.0);
p_source_geometry_data->add_mesh_array(arr, transform);
}
ConcavePolygonShape3D *concave_polygon = Object::cast_to<ConcavePolygonShape3D>(*s);
if (concave_polygon) {
p_source_geometry_data->add_faces(concave_polygon->get_faces(), transform);
}
ConvexPolygonShape3D *convex_polygon = Object::cast_to<ConvexPolygonShape3D>(*s);
if (convex_polygon) {
Vector<Vector3> varr = Variant(convex_polygon->get_points());
Geometry3D::MeshData md;
Error err = ConvexHullComputer::convex_hull(varr, md);
if (err == OK) {
PackedVector3Array faces;
for (const Geometry3D::MeshData::Face &face : md.faces) {
for (uint32_t k = 2; k < face.indices.size(); ++k) {
faces.push_back(md.vertices[face.indices[0]]);
faces.push_back(md.vertices[face.indices[k - 1]]);
faces.push_back(md.vertices[face.indices[k]]);
}
}
p_source_geometry_data->add_faces(faces, transform);
}
}
HeightMapShape3D *heightmap_shape = Object::cast_to<HeightMapShape3D>(*s);
if (heightmap_shape) {
int heightmap_depth = heightmap_shape->get_map_depth();
int heightmap_width = heightmap_shape->get_map_width();
if (heightmap_depth >= 2 && heightmap_width >= 2) {
const Vector<real_t> &map_data = heightmap_shape->get_map_data();
Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1);
Vector3 start = Vector3(heightmap_gridsize.x, 0, heightmap_gridsize.y) * -0.5;
Vector<Vector3> vertex_array;
vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6);
Vector3 *vertex_array_ptrw = vertex_array.ptrw();
const real_t *map_data_ptr = map_data.ptr();
int vertex_index = 0;
for (int d = 0; d < heightmap_depth - 1; d++) {
for (int w = 0; w < heightmap_width - 1; w++) {
vertex_array_ptrw[vertex_index] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + w], d);
vertex_array_ptrw[vertex_index + 1] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 2] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_array_ptrw[vertex_index + 3] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + w + 1], d);
vertex_array_ptrw[vertex_index + 4] = start + Vector3(w + 1, map_data_ptr[(heightmap_width * d) + heightmap_width + w + 1], d + 1);
vertex_array_ptrw[vertex_index + 5] = start + Vector3(w, map_data_ptr[(heightmap_width * d) + heightmap_width + w], d + 1);
vertex_index += 6;
}
}
if (vertex_array.size() > 0) {
p_source_geometry_data->add_faces(vertex_array, transform);
}
}
}
}
}
}
}
void StaticBody3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("set_constant_linear_velocity", "vel"), &StaticBody3D::set_constant_linear_velocity);
ClassDB::bind_method(D_METHOD("set_constant_angular_velocity", "vel"), &StaticBody3D::set_constant_angular_velocity);

10
scene/3d/physics/static_body_3d.h
View file

@ -33,6 +33,9 @@
#include "scene/3d/physics/physics_body_3d.h"
class NavigationMesh;
class NavigationMeshSourceGeometryData3D;
class StaticBody3D : public PhysicsBody3D {
GDCLASS(StaticBody3D, PhysicsBody3D);
@ -59,6 +62,13 @@ public:
private:
void _reload_physics_characteristics();
static Callable _navmesh_source_geometry_parsing_callback;
static RID _navmesh_source_geometry_parser;
public:
static void navmesh_parse_init();
static void navmesh_parse_source_geometry(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node);
};
#endif // STATIC_BODY_3D_H

18
scene/register_scene_types.cpp
View file

@ -1040,6 +1040,24 @@ void register_scene_types() {
OS::get_singleton()->yield(); // may take time to init
// 2D nodes that support navmesh baking need to server register their source geometry parsers.
MeshInstance2D::navmesh_parse_init();
MultiMeshInstance2D::navmesh_parse_init();
NavigationObstacle2D::navmesh_parse_init();
Polygon2D::navmesh_parse_init();
TileMap::navmesh_parse_init();
TileMapLayer::navmesh_parse_init();
StaticBody2D::navmesh_parse_init();
#ifndef _3D_DISABLED
// 3D nodes that support navmesh baking need to server register their source geometry parsers.
MeshInstance3D::navmesh_parse_init();
MultiMeshInstance3D::navmesh_parse_init();
NavigationObstacle3D::navmesh_parse_init();
StaticBody3D::navmesh_parse_init();
#endif
OS::get_singleton()->yield(); // may take time to init
GDREGISTER_ABSTRACT_CLASS(SceneState);
GDREGISTER_CLASS(PackedScene);

71
servers/navigation_server_2d.cpp
View file

@ -31,10 +31,15 @@
#include "navigation_server_2d.h"
#include "navigation_server_2d.compat.inc"
#include "servers/navigation_server_2d_dummy.h"
#include "servers/navigation_server_3d.h"
NavigationServer2D *NavigationServer2D::singleton = nullptr;
RWLock NavigationServer2D::geometry_parser_rwlock;
RID_Owner<NavMeshGeometryParser2D> NavigationServer2D::geometry_parser_owner;
LocalVector<NavMeshGeometryParser2D *> NavigationServer2D::generator_parsers;
void NavigationServer2D::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_maps"), &NavigationServer2D::get_maps);
@ -208,6 +213,47 @@ void NavigationServer2D::_emit_navigation_debug_changed_signal() {
NavigationServer2D::~NavigationServer2D() {
singleton = nullptr;
RWLockWrite write_lock(geometry_parser_rwlock);
for (NavMeshGeometryParser2D *parser : generator_parsers) {
geometry_parser_owner.free(parser->self);
}
generator_parsers.clear();
}
RID NavigationServer2D::source_geometry_parser_create() {
RWLockWrite write_lock(geometry_parser_rwlock);
RID rid = geometry_parser_owner.make_rid();
NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(rid);
parser->self = rid;
generator_parsers.push_back(parser);
return rid;
}
void NavigationServer2D::free(RID p_object) {
if (!geometry_parser_owner.owns(p_object)) {
return;
}
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_object);
ERR_FAIL_NULL(parser);
generator_parsers.erase(parser);
geometry_parser_owner.free(parser->self);
}
void NavigationServer2D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_parser);
ERR_FAIL_NULL(parser);
parser->callback = p_callback;
}
void NavigationServer2D::_emit_map_changed(RID p_map) {
@ -418,6 +464,8 @@ bool NavigationServer2D::get_debug_navigation_avoidance_enable_obstacles_static(
///////////////////////////////////////////////////////
static NavigationServer2D *navigation_server_2d = nullptr;
NavigationServer2DCallback NavigationServer2DManager::create_callback = nullptr;
void NavigationServer2DManager::set_default_server(NavigationServer2DCallback p_callback) {
@ -431,3 +479,26 @@ NavigationServer2D *NavigationServer2DManager::new_default_server() {
return create_callback();
}
void NavigationServer2DManager::initialize_server() {
// NavigationServer3D must be initialized before NavigationServer2D.
ERR_FAIL_NULL(NavigationServer3D::get_singleton());
ERR_FAIL_COND(navigation_server_2d != nullptr);
// Init 2D Navigation Server
navigation_server_2d = NavigationServer2DManager::new_default_server();
if (!navigation_server_2d) {
WARN_VERBOSE("Failed to initialize NavigationServer2D. Fall back to dummy server.");
navigation_server_2d = memnew(NavigationServer2DDummy);
}
ERR_FAIL_NULL_MSG(navigation_server_2d, "Failed to initialize NavigationServer2D.");
navigation_server_2d->init();
}
void NavigationServer2DManager::finalize_server() {
ERR_FAIL_NULL(navigation_server_2d);
navigation_server_2d->finish();
memdelete(navigation_server_2d);
navigation_server_2d = nullptr;
}

14
servers/navigation_server_2d.h
View file

@ -43,6 +43,11 @@
class NavMeshGenerator2D;
#endif // CLIPPER2_ENABLED
struct NavMeshGeometryParser2D {
RID self;
Callable callback;
};
// This server exposes the `NavigationServer3D` features in the 2D world.
class NavigationServer2D : public Object {
GDCLASS(NavigationServer2D, Object);
@ -311,6 +316,12 @@ public:
virtual void bake_from_source_geometry_data_async(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback = Callable()) = 0;
virtual bool is_baking_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon) const = 0;
protected:
static RWLock geometry_parser_rwlock;
static RID_Owner<NavMeshGeometryParser2D> geometry_parser_owner;
static LocalVector<NavMeshGeometryParser2D *> generator_parsers;
public:
virtual RID source_geometry_parser_create() = 0;
virtual void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) = 0;
@ -419,6 +430,9 @@ class NavigationServer2DManager {
public:
static void set_default_server(NavigationServer2DCallback p_callback);
static NavigationServer2D *new_default_server();
static void initialize_server();
static void finalize_server();
};
#endif // NAVIGATION_SERVER_2D_H

72
servers/navigation_server_3d.cpp
View file

@ -34,9 +34,14 @@
#include "core/config/project_settings.h"
#include "scene/main/node.h"
#include "servers/navigation/navigation_globals.h"
#include "servers/navigation_server_3d_dummy.h"
NavigationServer3D *NavigationServer3D::singleton = nullptr;
RWLock NavigationServer3D::geometry_parser_rwlock;
RID_Owner<NavMeshGeometryParser3D> NavigationServer3D::geometry_parser_owner;
LocalVector<NavMeshGeometryParser3D *> NavigationServer3D::generator_parsers;
void NavigationServer3D::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_maps"), &NavigationServer3D::get_maps);
@ -303,6 +308,47 @@ NavigationServer3D::NavigationServer3D() {
NavigationServer3D::~NavigationServer3D() {
singleton = nullptr;
RWLockWrite write_lock(geometry_parser_rwlock);
for (NavMeshGeometryParser3D *parser : generator_parsers) {
geometry_parser_owner.free(parser->self);
}
generator_parsers.clear();
}
RID NavigationServer3D::source_geometry_parser_create() {
RWLockWrite write_lock(geometry_parser_rwlock);
RID rid = geometry_parser_owner.make_rid();
NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(rid);
parser->self = rid;
generator_parsers.push_back(parser);
return rid;
}
void NavigationServer3D::free(RID p_object) {
if (!geometry_parser_owner.owns(p_object)) {
return;
}
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(p_object);
ERR_FAIL_NULL(parser);
generator_parsers.erase(parser);
geometry_parser_owner.free(parser->self);
}
void NavigationServer3D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
RWLockWrite write_lock(geometry_parser_rwlock);
NavMeshGeometryParser3D *parser = geometry_parser_owner.get_or_null(p_parser);
ERR_FAIL_NULL(parser);
parser->callback = p_callback;
}
void NavigationServer3D::set_debug_enabled(bool p_enabled) {
@ -945,6 +991,8 @@ bool NavigationServer3D::get_debug_avoidance_enabled() const {
///////////////////////////////////////////////////////
static NavigationServer3D *navigation_server_3d = nullptr;
NavigationServer3DCallback NavigationServer3DManager::create_callback = nullptr;
void NavigationServer3DManager::set_default_server(NavigationServer3DCallback p_callback) {
@ -958,3 +1006,27 @@ NavigationServer3D *NavigationServer3DManager::new_default_server() {
return create_callback();
}
void NavigationServer3DManager::initialize_server() {
ERR_FAIL_COND(navigation_server_3d != nullptr);
// Init 3D Navigation Server
navigation_server_3d = NavigationServer3DManager::new_default_server();
// Fall back to dummy if no default server has been registered.
if (!navigation_server_3d) {
WARN_VERBOSE("Failed to initialize NavigationServer3D. Fall back to dummy server.");
navigation_server_3d = memnew(NavigationServer3DDummy);
}
// Should be impossible, but make sure it's not null.
ERR_FAIL_NULL_MSG(navigation_server_3d, "Failed to initialize NavigationServer3D.");
navigation_server_3d->init();
}
void NavigationServer3DManager::finalize_server() {
ERR_FAIL_NULL(navigation_server_3d);
navigation_server_3d->finish();
memdelete(navigation_server_3d);
navigation_server_3d = nullptr;
}

14
servers/navigation_server_3d.h
View file

@ -39,6 +39,11 @@
#include "servers/navigation/navigation_path_query_parameters_3d.h"
#include "servers/navigation/navigation_path_query_result_3d.h"
struct NavMeshGeometryParser3D {
RID self;
Callable callback;
};
/// This server uses the concept of internal mutability.
/// All the constant functions can be called in multithread because internally
/// the server takes care to schedule the functions access.
@ -357,6 +362,12 @@ public:
virtual bool is_baking_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) const = 0;
#endif // _3D_DISABLED
protected:
static RWLock geometry_parser_rwlock;
static RID_Owner<NavMeshGeometryParser3D> geometry_parser_owner;
static LocalVector<NavMeshGeometryParser3D *> generator_parsers;
public:
virtual RID source_geometry_parser_create() = 0;
virtual void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) = 0;
@ -573,6 +584,9 @@ class NavigationServer3DManager {
public:
static void set_default_server(NavigationServer3DCallback p_callback);
static NavigationServer3D *new_default_server();
static void initialize_server();
static void finalize_server();
};
VARIANT_ENUM_CAST(NavigationServer3D::ProcessInfo);