1/**************************************************************************/
2/* nav_mesh_generator_3d.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#ifndef _3D_DISABLED
32
33#include "nav_mesh_generator_3d.h"
34
35#include "core/config/project_settings.h"
36#include "core/math/convex_hull.h"
37#include "core/os/thread.h"
38#include "scene/3d/mesh_instance_3d.h"
39#include "scene/3d/multimesh_instance_3d.h"
40#include "scene/3d/physics_body_3d.h"
41#include "scene/resources/box_shape_3d.h"
42#include "scene/resources/capsule_shape_3d.h"
43#include "scene/resources/concave_polygon_shape_3d.h"
44#include "scene/resources/convex_polygon_shape_3d.h"
45#include "scene/resources/cylinder_shape_3d.h"
46#include "scene/resources/height_map_shape_3d.h"
47#include "scene/resources/navigation_mesh.h"
48#include "scene/resources/navigation_mesh_source_geometry_data_3d.h"
49#include "scene/resources/primitive_meshes.h"
50#include "scene/resources/shape_3d.h"
51#include "scene/resources/sphere_shape_3d.h"
52#include "scene/resources/world_boundary_shape_3d.h"
53
54#include "modules/modules_enabled.gen.h" // For csg, gridmap.
55
56#ifdef MODULE_CSG_ENABLED
57#include "modules/csg/csg_shape.h"
58#endif
59#ifdef MODULE_GRIDMAP_ENABLED
60#include "modules/gridmap/grid_map.h"
61#endif
62
63#include <Recast.h>
64
65NavMeshGenerator3D *NavMeshGenerator3D::singleton = nullptr;
66Mutex NavMeshGenerator3D::baking_navmesh_mutex;
67Mutex NavMeshGenerator3D::generator_task_mutex;
68bool NavMeshGenerator3D::use_threads = true;
69bool NavMeshGenerator3D::baking_use_multiple_threads = true;
70bool NavMeshGenerator3D::baking_use_high_priority_threads = true;
71HashSet<Ref<NavigationMesh>> NavMeshGenerator3D::baking_navmeshes;
72HashMap<WorkerThreadPool::TaskID, NavMeshGenerator3D::NavMeshGeneratorTask3D *> NavMeshGenerator3D::generator_tasks;
73
74NavMeshGenerator3D *NavMeshGenerator3D::get_singleton() {
75 return singleton;
76}
77
78NavMeshGenerator3D::NavMeshGenerator3D() {
79 ERR_FAIL_COND(singleton != nullptr);
80 singleton = this;
81
82 baking_use_multiple_threads = GLOBAL_GET("navigation/baking/thread_model/baking_use_multiple_threads");
83 baking_use_high_priority_threads = GLOBAL_GET("navigation/baking/thread_model/baking_use_high_priority_threads");
84
85 // Using threads might cause problems on certain exports or with the Editor on certain devices.
86 // This is the main switch to turn threaded navmesh baking off should the need arise.
87 use_threads = baking_use_multiple_threads && !Engine::get_singleton()->is_editor_hint();
88}
89
90NavMeshGenerator3D::~NavMeshGenerator3D() {
91 cleanup();
92}
93
94void NavMeshGenerator3D::sync() {
95 if (generator_tasks.size() == 0) {
96 return;
97 }
98
99 baking_navmesh_mutex.lock();
100 generator_task_mutex.lock();
101
102 LocalVector<WorkerThreadPool::TaskID> finished_task_ids;
103
104 for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) {
105 if (WorkerThreadPool::get_singleton()->is_task_completed(E.key)) {
106 WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
107 finished_task_ids.push_back(E.key);
108
109 NavMeshGeneratorTask3D *generator_task = E.value;
110 DEV_ASSERT(generator_task->status == NavMeshGeneratorTask3D::TaskStatus::BAKING_FINISHED);
111
112 baking_navmeshes.erase(generator_task->navigation_mesh);
113 if (generator_task->callback.is_valid()) {
114 generator_emit_callback(generator_task->callback);
115 }
116 memdelete(generator_task);
117 }
118 }
119
120 for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) {
121 generator_tasks.erase(finished_task_id);
122 }
123
124 generator_task_mutex.unlock();
125 baking_navmesh_mutex.unlock();
126}
127
128void NavMeshGenerator3D::cleanup() {
129 baking_navmesh_mutex.lock();
130 generator_task_mutex.lock();
131
132 baking_navmeshes.clear();
133
134 for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) {
135 WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
136 NavMeshGeneratorTask3D *generator_task = E.value;
137 memdelete(generator_task);
138 }
139 generator_tasks.clear();
140
141 generator_task_mutex.unlock();
142 baking_navmesh_mutex.unlock();
143}
144
145void NavMeshGenerator3D::finish() {
146 cleanup();
147}
148
149void NavMeshGenerator3D::parse_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
150 ERR_FAIL_COND(!Thread::is_main_thread());
151 ERR_FAIL_COND(!p_navigation_mesh.is_valid());
152 ERR_FAIL_COND(p_root_node == nullptr);
153 ERR_FAIL_COND(!p_root_node->is_inside_tree());
154 ERR_FAIL_COND(!p_source_geometry_data.is_valid());
155
156 generator_parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node);
157
158 if (p_callback.is_valid()) {
159 generator_emit_callback(p_callback);
160 }
161}
162
163void NavMeshGenerator3D::bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback) {
164 ERR_FAIL_COND(!p_navigation_mesh.is_valid());
165 ERR_FAIL_COND(!p_source_geometry_data.is_valid());
166
167 if (!p_source_geometry_data->has_data()) {
168 p_navigation_mesh->clear();
169 if (p_callback.is_valid()) {
170 generator_emit_callback(p_callback);
171 }
172 return;
173 }
174
175 baking_navmesh_mutex.lock();
176 if (baking_navmeshes.has(p_navigation_mesh)) {
177 baking_navmesh_mutex.unlock();
178 ERR_FAIL_MSG("NavigationMesh is already baking. Wait for current bake to finish.");
179 }
180 baking_navmeshes.insert(p_navigation_mesh);
181 baking_navmesh_mutex.unlock();
182
183 generator_bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data);
184
185 baking_navmesh_mutex.lock();
186 baking_navmeshes.erase(p_navigation_mesh);
187 baking_navmesh_mutex.unlock();
188
189 if (p_callback.is_valid()) {
190 generator_emit_callback(p_callback);
191 }
192}
193
194void NavMeshGenerator3D::bake_from_source_geometry_data_async(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback) {
195 ERR_FAIL_COND(!p_navigation_mesh.is_valid());
196 ERR_FAIL_COND(!p_source_geometry_data.is_valid());
197
198 if (!p_source_geometry_data->has_data()) {
199 p_navigation_mesh->clear();
200 if (p_callback.is_valid()) {
201 generator_emit_callback(p_callback);
202 }
203 return;
204 }
205
206 if (!use_threads) {
207 bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
208 return;
209 }
210
211 baking_navmesh_mutex.lock();
212 if (baking_navmeshes.has(p_navigation_mesh)) {
213 baking_navmesh_mutex.unlock();
214 ERR_FAIL_MSG("NavigationMesh is already baking. Wait for current bake to finish.");
215 return;
216 }
217 baking_navmeshes.insert(p_navigation_mesh);
218 baking_navmesh_mutex.unlock();
219
220 generator_task_mutex.lock();
221 NavMeshGeneratorTask3D *generator_task = memnew(NavMeshGeneratorTask3D);
222 generator_task->navigation_mesh = p_navigation_mesh;
223 generator_task->source_geometry_data = p_source_geometry_data;
224 generator_task->callback = p_callback;
225 generator_task->status = NavMeshGeneratorTask3D::TaskStatus::BAKING_STARTED;
226 generator_task->thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMeshGenerator3D::generator_thread_bake, generator_task, NavMeshGenerator3D::baking_use_high_priority_threads, SNAME("NavMeshGeneratorBake3D"));
227 generator_tasks.insert(generator_task->thread_task_id, generator_task);
228 generator_task_mutex.unlock();
229}
230
231void NavMeshGenerator3D::generator_thread_bake(void *p_arg) {
232 NavMeshGeneratorTask3D *generator_task = static_cast<NavMeshGeneratorTask3D *>(p_arg);
233
234 generator_bake_from_source_geometry_data(generator_task->navigation_mesh, generator_task->source_geometry_data);
235
236 generator_task->status = NavMeshGeneratorTask3D::TaskStatus::BAKING_FINISHED;
237}
238
239void NavMeshGenerator3D::generator_parse_geometry_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node, bool p_recurse_children) {
240 generator_parse_meshinstance3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
241 generator_parse_multimeshinstance3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
242 generator_parse_staticbody3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
243#ifdef MODULE_CSG_ENABLED
244 generator_parse_csgshape3d_node(p_navigation_mesh, p_source_geometry_data, p_node);
245#endif
246#ifdef MODULE_GRIDMAP_ENABLED
247 generator_parse_gridmap_node(p_navigation_mesh, p_source_geometry_data, p_node);
248#endif
249
250 if (p_recurse_children) {
251 for (int i = 0; i < p_node->get_child_count(); i++) {
252 generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, p_node->get_child(i), p_recurse_children);
253 }
254 }
255}
256
257void NavMeshGenerator3D::generator_parse_meshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
258 MeshInstance3D *mesh_instance = Object::cast_to<MeshInstance3D>(p_node);
259
260 if (mesh_instance) {
261 NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
262
263 if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
264 Ref<Mesh> mesh = mesh_instance->get_mesh();
265 if (mesh.is_valid()) {
266 p_source_geometry_data->add_mesh(mesh, mesh_instance->get_global_transform());
267 }
268 }
269 }
270}
271
272void NavMeshGenerator3D::generator_parse_multimeshinstance3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
273 MultiMeshInstance3D *multimesh_instance = Object::cast_to<MultiMeshInstance3D>(p_node);
274
275 if (multimesh_instance) {
276 NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
277
278 if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
279 Ref<MultiMesh> multimesh = multimesh_instance->get_multimesh();
280 if (multimesh.is_valid()) {
281 Ref<Mesh> mesh = multimesh->get_mesh();
282 if (mesh.is_valid()) {
283 int n = multimesh->get_visible_instance_count();
284 if (n == -1) {
285 n = multimesh->get_instance_count();
286 }
287 for (int i = 0; i < n; i++) {
288 p_source_geometry_data->add_mesh(mesh, multimesh_instance->get_global_transform() * multimesh->get_instance_transform(i));
289 }
290 }
291 }
292 }
293 }
294}
295
296void NavMeshGenerator3D::generator_parse_staticbody3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
297 StaticBody3D *static_body = Object::cast_to<StaticBody3D>(p_node);
298
299 if (static_body) {
300 NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
301 uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
302
303 if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (static_body->get_collision_layer() & parsed_collision_mask)) {
304 List<uint32_t> shape_owners;
305 static_body->get_shape_owners(&shape_owners);
306 for (uint32_t shape_owner : shape_owners) {
307 if (static_body->is_shape_owner_disabled(shape_owner)) {
308 continue;
309 }
310 const int shape_count = static_body->shape_owner_get_shape_count(shape_owner);
311 for (int shape_index = 0; shape_index < shape_count; shape_index++) {
312 Ref<Shape3D> s = static_body->shape_owner_get_shape(shape_owner, shape_index);
313 if (s.is_null()) {
314 continue;
315 }
316
317 const Transform3D transform = static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner);
318
319 BoxShape3D *box = Object::cast_to<BoxShape3D>(*s);
320 if (box) {
321 Array arr;
322 arr.resize(RS::ARRAY_MAX);
323 BoxMesh::create_mesh_array(arr, box->get_size());
324 p_source_geometry_data->add_mesh_array(arr, transform);
325 }
326
327 CapsuleShape3D *capsule = Object::cast_to<CapsuleShape3D>(*s);
328 if (capsule) {
329 Array arr;
330 arr.resize(RS::ARRAY_MAX);
331 CapsuleMesh::create_mesh_array(arr, capsule->get_radius(), capsule->get_height());
332 p_source_geometry_data->add_mesh_array(arr, transform);
333 }
334
335 CylinderShape3D *cylinder = Object::cast_to<CylinderShape3D>(*s);
336 if (cylinder) {
337 Array arr;
338 arr.resize(RS::ARRAY_MAX);
339 CylinderMesh::create_mesh_array(arr, cylinder->get_radius(), cylinder->get_radius(), cylinder->get_height());
340 p_source_geometry_data->add_mesh_array(arr, transform);
341 }
342
343 SphereShape3D *sphere = Object::cast_to<SphereShape3D>(*s);
344 if (sphere) {
345 Array arr;
346 arr.resize(RS::ARRAY_MAX);
347 SphereMesh::create_mesh_array(arr, sphere->get_radius(), sphere->get_radius() * 2.0);
348 p_source_geometry_data->add_mesh_array(arr, transform);
349 }
350
351 ConcavePolygonShape3D *concave_polygon = Object::cast_to<ConcavePolygonShape3D>(*s);
352 if (concave_polygon) {
353 p_source_geometry_data->add_faces(concave_polygon->get_faces(), transform);
354 }
355
356 ConvexPolygonShape3D *convex_polygon = Object::cast_to<ConvexPolygonShape3D>(*s);
357 if (convex_polygon) {
358 Vector<Vector3> varr = Variant(convex_polygon->get_points());
359 Geometry3D::MeshData md;
360
361 Error err = ConvexHullComputer::convex_hull(varr, md);
362
363 if (err == OK) {
364 PackedVector3Array faces;
365
366 for (const Geometry3D::MeshData::Face &face : md.faces) {
367 for (uint32_t k = 2; k < face.indices.size(); ++k) {
368 faces.push_back(md.vertices[face.indices[0]]);
369 faces.push_back(md.vertices[face.indices[k - 1]]);
370 faces.push_back(md.vertices[face.indices[k]]);
371 }
372 }
373
374 p_source_geometry_data->add_faces(faces, transform);
375 }
376 }
377
378 HeightMapShape3D *heightmap_shape = Object::cast_to<HeightMapShape3D>(*s);
379 if (heightmap_shape) {
380 int heightmap_depth = heightmap_shape->get_map_depth();
381 int heightmap_width = heightmap_shape->get_map_width();
382
383 if (heightmap_depth >= 2 && heightmap_width >= 2) {
384 const Vector<real_t> &map_data = heightmap_shape->get_map_data();
385
386 Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1);
387 Vector2 start = heightmap_gridsize * -0.5;
388
389 Vector<Vector3> vertex_array;
390 vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6);
391 int map_data_current_index = 0;
392
393 for (int d = 0; d < heightmap_depth; d++) {
394 for (int w = 0; w < heightmap_width; w++) {
395 if (map_data_current_index + 1 + heightmap_depth < map_data.size()) {
396 float top_left_height = map_data[map_data_current_index];
397 float top_right_height = map_data[map_data_current_index + 1];
398 float bottom_left_height = map_data[map_data_current_index + heightmap_depth];
399 float bottom_right_height = map_data[map_data_current_index + 1 + heightmap_depth];
400
401 Vector3 top_left = Vector3(start.x + w, top_left_height, start.y + d);
402 Vector3 top_right = Vector3(start.x + w + 1.0, top_right_height, start.y + d);
403 Vector3 bottom_left = Vector3(start.x + w, bottom_left_height, start.y + d + 1.0);
404 Vector3 bottom_right = Vector3(start.x + w + 1.0, bottom_right_height, start.y + d + 1.0);
405
406 vertex_array.push_back(top_right);
407 vertex_array.push_back(bottom_left);
408 vertex_array.push_back(top_left);
409 vertex_array.push_back(top_right);
410 vertex_array.push_back(bottom_right);
411 vertex_array.push_back(bottom_left);
412 }
413 map_data_current_index += 1;
414 }
415 }
416 if (vertex_array.size() > 0) {
417 p_source_geometry_data->add_faces(vertex_array, transform);
418 }
419 }
420 }
421 }
422 }
423 }
424 }
425}
426
427#ifdef MODULE_CSG_ENABLED
428void NavMeshGenerator3D::generator_parse_csgshape3d_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
429 CSGShape3D *csgshape3d = Object::cast_to<CSGShape3D>(p_node);
430
431 if (csgshape3d) {
432 NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
433 uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
434
435 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) {
436 CSGShape3D *csg_shape = Object::cast_to<CSGShape3D>(p_node);
437 Array meshes = csg_shape->get_meshes();
438 if (!meshes.is_empty()) {
439 Ref<Mesh> mesh = meshes[1];
440 if (mesh.is_valid()) {
441 p_source_geometry_data->add_mesh(mesh, csg_shape->get_global_transform());
442 }
443 }
444 }
445 }
446}
447#endif // MODULE_CSG_ENABLED
448
449#ifdef MODULE_GRIDMAP_ENABLED
450void NavMeshGenerator3D::generator_parse_gridmap_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node) {
451 GridMap *gridmap = Object::cast_to<GridMap>(p_node);
452
453 if (gridmap) {
454 NavigationMesh::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
455 uint32_t parsed_collision_mask = p_navigation_mesh->get_collision_mask();
456
457 if (parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) {
458 Array meshes = gridmap->get_meshes();
459 Transform3D xform = gridmap->get_global_transform();
460 for (int i = 0; i < meshes.size(); i += 2) {
461 Ref<Mesh> mesh = meshes[i + 1];
462 if (mesh.is_valid()) {
463 p_source_geometry_data->add_mesh(mesh, xform * (Transform3D)meshes[i]);
464 }
465 }
466 }
467
468 else if ((parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationMesh::PARSED_GEOMETRY_BOTH) && (gridmap->get_collision_layer() & parsed_collision_mask)) {
469 Array shapes = gridmap->get_collision_shapes();
470 for (int i = 0; i < shapes.size(); i += 2) {
471 RID shape = shapes[i + 1];
472 PhysicsServer3D::ShapeType type = PhysicsServer3D::get_singleton()->shape_get_type(shape);
473 Variant data = PhysicsServer3D::get_singleton()->shape_get_data(shape);
474
475 switch (type) {
476 case PhysicsServer3D::SHAPE_SPHERE: {
477 real_t radius = data;
478 Array arr;
479 arr.resize(RS::ARRAY_MAX);
480 SphereMesh::create_mesh_array(arr, radius, radius * 2.0);
481 p_source_geometry_data->add_mesh_array(arr, shapes[i]);
482 } break;
483 case PhysicsServer3D::SHAPE_BOX: {
484 Vector3 extents = data;
485 Array arr;
486 arr.resize(RS::ARRAY_MAX);
487 BoxMesh::create_mesh_array(arr, extents * 2.0);
488 p_source_geometry_data->add_mesh_array(arr, shapes[i]);
489 } break;
490 case PhysicsServer3D::SHAPE_CAPSULE: {
491 Dictionary dict = data;
492 real_t radius = dict["radius"];
493 real_t height = dict["height"];
494 Array arr;
495 arr.resize(RS::ARRAY_MAX);
496 CapsuleMesh::create_mesh_array(arr, radius, height);
497 p_source_geometry_data->add_mesh_array(arr, shapes[i]);
498 } break;
499 case PhysicsServer3D::SHAPE_CYLINDER: {
500 Dictionary dict = data;
501 real_t radius = dict["radius"];
502 real_t height = dict["height"];
503 Array arr;
504 arr.resize(RS::ARRAY_MAX);
505 CylinderMesh::create_mesh_array(arr, radius, radius, height);
506 p_source_geometry_data->add_mesh_array(arr, shapes[i]);
507 } break;
508 case PhysicsServer3D::SHAPE_CONVEX_POLYGON: {
509 PackedVector3Array vertices = data;
510 Geometry3D::MeshData md;
511
512 Error err = ConvexHullComputer::convex_hull(vertices, md);
513
514 if (err == OK) {
515 PackedVector3Array faces;
516
517 for (const Geometry3D::MeshData::Face &face : md.faces) {
518 for (uint32_t k = 2; k < face.indices.size(); ++k) {
519 faces.push_back(md.vertices[face.indices[0]]);
520 faces.push_back(md.vertices[face.indices[k - 1]]);
521 faces.push_back(md.vertices[face.indices[k]]);
522 }
523 }
524
525 p_source_geometry_data->add_faces(faces, shapes[i]);
526 }
527 } break;
528 case PhysicsServer3D::SHAPE_CONCAVE_POLYGON: {
529 Dictionary dict = data;
530 PackedVector3Array faces = Variant(dict["faces"]);
531 p_source_geometry_data->add_faces(faces, shapes[i]);
532 } break;
533 case PhysicsServer3D::SHAPE_HEIGHTMAP: {
534 Dictionary dict = data;
535 ///< dict( int:"width", int:"depth",float:"cell_size", float_array:"heights"
536 int heightmap_depth = dict["depth"];
537 int heightmap_width = dict["width"];
538
539 if (heightmap_depth >= 2 && heightmap_width >= 2) {
540 const Vector<real_t> &map_data = dict["heights"];
541
542 Vector2 heightmap_gridsize(heightmap_width - 1, heightmap_depth - 1);
543 Vector2 start = heightmap_gridsize * -0.5;
544
545 Vector<Vector3> vertex_array;
546 vertex_array.resize((heightmap_depth - 1) * (heightmap_width - 1) * 6);
547 int map_data_current_index = 0;
548
549 for (int d = 0; d < heightmap_depth; d++) {
550 for (int w = 0; w < heightmap_width; w++) {
551 if (map_data_current_index + 1 + heightmap_depth < map_data.size()) {
552 float top_left_height = map_data[map_data_current_index];
553 float top_right_height = map_data[map_data_current_index + 1];
554 float bottom_left_height = map_data[map_data_current_index + heightmap_depth];
555 float bottom_right_height = map_data[map_data_current_index + 1 + heightmap_depth];
556
557 Vector3 top_left = Vector3(start.x + w, top_left_height, start.y + d);
558 Vector3 top_right = Vector3(start.x + w + 1.0, top_right_height, start.y + d);
559 Vector3 bottom_left = Vector3(start.x + w, bottom_left_height, start.y + d + 1.0);
560 Vector3 bottom_right = Vector3(start.x + w + 1.0, bottom_right_height, start.y + d + 1.0);
561
562 vertex_array.push_back(top_right);
563 vertex_array.push_back(bottom_left);
564 vertex_array.push_back(top_left);
565 vertex_array.push_back(top_right);
566 vertex_array.push_back(bottom_right);
567 vertex_array.push_back(bottom_left);
568 }
569 map_data_current_index += 1;
570 }
571 }
572 if (vertex_array.size() > 0) {
573 p_source_geometry_data->add_faces(vertex_array, shapes[i]);
574 }
575 }
576 } break;
577 default: {
578 WARN_PRINT("Unsupported collision shape type.");
579 } break;
580 }
581 }
582 }
583 }
584}
585#endif // MODULE_GRIDMAP_ENABLED
586
587void NavMeshGenerator3D::generator_parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node) {
588 List<Node *> parse_nodes;
589
590 if (p_navigation_mesh->get_source_geometry_mode() == NavigationMesh::SOURCE_GEOMETRY_ROOT_NODE_CHILDREN) {
591 parse_nodes.push_back(p_root_node);
592 } else {
593 p_root_node->get_tree()->get_nodes_in_group(p_navigation_mesh->get_source_group_name(), &parse_nodes);
594 }
595
596 Transform3D root_node_transform = Transform3D();
597 if (Object::cast_to<Node3D>(p_root_node)) {
598 root_node_transform = Object::cast_to<Node3D>(p_root_node)->get_global_transform().affine_inverse();
599 }
600
601 p_source_geometry_data->clear();
602 p_source_geometry_data->root_node_transform = root_node_transform;
603
604 bool recurse_children = p_navigation_mesh->get_source_geometry_mode() != NavigationMesh::SOURCE_GEOMETRY_GROUPS_EXPLICIT;
605
606 for (Node *parse_node : parse_nodes) {
607 generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, parse_node, recurse_children);
608 }
609};
610
611void NavMeshGenerator3D::generator_bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data) {
612 if (p_navigation_mesh.is_null() || p_source_geometry_data.is_null()) {
613 return;
614 }
615
616 const Vector<float> &vertices = p_source_geometry_data->get_vertices();
617 const Vector<int> &indices = p_source_geometry_data->get_indices();
618
619 if (vertices.size() < 3 || indices.size() < 3) {
620 return;
621 }
622
623 rcHeightfield *hf = nullptr;
624 rcCompactHeightfield *chf = nullptr;
625 rcContourSet *cset = nullptr;
626 rcPolyMesh *poly_mesh = nullptr;
627 rcPolyMeshDetail *detail_mesh = nullptr;
628 rcContext ctx;
629
630 // added to keep track of steps, no functionality right now
631 String bake_state = "";
632
633 bake_state = "Setting up Configuration..."; // step #1
634
635 const float *verts = vertices.ptr();
636 const int nverts = vertices.size() / 3;
637 const int *tris = indices.ptr();
638 const int ntris = indices.size() / 3;
639
640 float bmin[3], bmax[3];
641 rcCalcBounds(verts, nverts, bmin, bmax);
642
643 rcConfig cfg;
644 memset(&cfg, 0, sizeof(cfg));
645
646 cfg.cs = p_navigation_mesh->get_cell_size();
647 cfg.ch = p_navigation_mesh->get_cell_height();
648 cfg.walkableSlopeAngle = p_navigation_mesh->get_agent_max_slope();
649 cfg.walkableHeight = (int)Math::ceil(p_navigation_mesh->get_agent_height() / cfg.ch);
650 cfg.walkableClimb = (int)Math::floor(p_navigation_mesh->get_agent_max_climb() / cfg.ch);
651 cfg.walkableRadius = (int)Math::ceil(p_navigation_mesh->get_agent_radius() / cfg.cs);
652 cfg.maxEdgeLen = (int)(p_navigation_mesh->get_edge_max_length() / p_navigation_mesh->get_cell_size());
653 cfg.maxSimplificationError = p_navigation_mesh->get_edge_max_error();
654 cfg.minRegionArea = (int)(p_navigation_mesh->get_region_min_size() * p_navigation_mesh->get_region_min_size());
655 cfg.mergeRegionArea = (int)(p_navigation_mesh->get_region_merge_size() * p_navigation_mesh->get_region_merge_size());
656 cfg.maxVertsPerPoly = (int)p_navigation_mesh->get_vertices_per_polygon();
657 cfg.detailSampleDist = MAX(p_navigation_mesh->get_cell_size() * p_navigation_mesh->get_detail_sample_distance(), 0.1f);
658 cfg.detailSampleMaxError = p_navigation_mesh->get_cell_height() * p_navigation_mesh->get_detail_sample_max_error();
659
660 if (!Math::is_equal_approx((float)cfg.walkableHeight * cfg.ch, p_navigation_mesh->get_agent_height())) {
661 WARN_PRINT("Property agent_height is ceiled to cell_height voxel units and loses precision.");
662 }
663 if (!Math::is_equal_approx((float)cfg.walkableClimb * cfg.ch, p_navigation_mesh->get_agent_max_climb())) {
664 WARN_PRINT("Property agent_max_climb is floored to cell_height voxel units and loses precision.");
665 }
666 if (!Math::is_equal_approx((float)cfg.walkableRadius * cfg.cs, p_navigation_mesh->get_agent_radius())) {
667 WARN_PRINT("Property agent_radius is ceiled to cell_size voxel units and loses precision.");
668 }
669 if (!Math::is_equal_approx((float)cfg.maxEdgeLen * cfg.cs, p_navigation_mesh->get_edge_max_length())) {
670 WARN_PRINT("Property edge_max_length is rounded to cell_size voxel units and loses precision.");
671 }
672 if (!Math::is_equal_approx((float)cfg.minRegionArea, p_navigation_mesh->get_region_min_size() * p_navigation_mesh->get_region_min_size())) {
673 WARN_PRINT("Property region_min_size is converted to int and loses precision.");
674 }
675 if (!Math::is_equal_approx((float)cfg.mergeRegionArea, p_navigation_mesh->get_region_merge_size() * p_navigation_mesh->get_region_merge_size())) {
676 WARN_PRINT("Property region_merge_size is converted to int and loses precision.");
677 }
678 if (!Math::is_equal_approx((float)cfg.maxVertsPerPoly, p_navigation_mesh->get_vertices_per_polygon())) {
679 WARN_PRINT("Property vertices_per_polygon is converted to int and loses precision.");
680 }
681 if (p_navigation_mesh->get_cell_size() * p_navigation_mesh->get_detail_sample_distance() < 0.1f) {
682 WARN_PRINT("Property detail_sample_distance is clamped to 0.1 world units as the resulting value from multiplying with cell_size is too low.");
683 }
684
685 cfg.bmin[0] = bmin[0];
686 cfg.bmin[1] = bmin[1];
687 cfg.bmin[2] = bmin[2];
688 cfg.bmax[0] = bmax[0];
689 cfg.bmax[1] = bmax[1];
690 cfg.bmax[2] = bmax[2];
691
692 AABB baking_aabb = p_navigation_mesh->get_filter_baking_aabb();
693 if (baking_aabb.has_volume()) {
694 Vector3 baking_aabb_offset = p_navigation_mesh->get_filter_baking_aabb_offset();
695 cfg.bmin[0] = baking_aabb.position[0] + baking_aabb_offset.x;
696 cfg.bmin[1] = baking_aabb.position[1] + baking_aabb_offset.y;
697 cfg.bmin[2] = baking_aabb.position[2] + baking_aabb_offset.z;
698 cfg.bmax[0] = cfg.bmin[0] + baking_aabb.size[0];
699 cfg.bmax[1] = cfg.bmin[1] + baking_aabb.size[1];
700 cfg.bmax[2] = cfg.bmin[2] + baking_aabb.size[2];
701 }
702
703 bake_state = "Calculating grid size..."; // step #2
704 rcCalcGridSize(cfg.bmin, cfg.bmax, cfg.cs, &cfg.width, &cfg.height);
705
706 // ~30000000 seems to be around sweetspot where Editor baking breaks
707 if ((cfg.width * cfg.height) > 30000000) {
708 WARN_PRINT("NavigationMesh baking process will likely fail."
709 "\nSource geometry is suspiciously big for the current Cell Size and Cell Height in the NavMesh Resource bake settings."
710 "\nIf baking does not fail, the resulting NavigationMesh will create serious pathfinding performance issues."
711 "\nIt is advised to increase Cell Size and/or Cell Height in the NavMesh Resource bake settings or reduce the size / scale of the source geometry.");
712 }
713
714 bake_state = "Creating heightfield..."; // step #3
715 hf = rcAllocHeightfield();
716
717 ERR_FAIL_COND(!hf);
718 ERR_FAIL_COND(!rcCreateHeightfield(&ctx, *hf, cfg.width, cfg.height, cfg.bmin, cfg.bmax, cfg.cs, cfg.ch));
719
720 bake_state = "Marking walkable triangles..."; // step #4
721 {
722 Vector<unsigned char> tri_areas;
723 tri_areas.resize(ntris);
724
725 ERR_FAIL_COND(tri_areas.size() == 0);
726
727 memset(tri_areas.ptrw(), 0, ntris * sizeof(unsigned char));
728 rcMarkWalkableTriangles(&ctx, cfg.walkableSlopeAngle, verts, nverts, tris, ntris, tri_areas.ptrw());
729
730 ERR_FAIL_COND(!rcRasterizeTriangles(&ctx, verts, nverts, tris, tri_areas.ptr(), ntris, *hf, cfg.walkableClimb));
731 }
732
733 if (p_navigation_mesh->get_filter_low_hanging_obstacles()) {
734 rcFilterLowHangingWalkableObstacles(&ctx, cfg.walkableClimb, *hf);
735 }
736 if (p_navigation_mesh->get_filter_ledge_spans()) {
737 rcFilterLedgeSpans(&ctx, cfg.walkableHeight, cfg.walkableClimb, *hf);
738 }
739 if (p_navigation_mesh->get_filter_walkable_low_height_spans()) {
740 rcFilterWalkableLowHeightSpans(&ctx, cfg.walkableHeight, *hf);
741 }
742
743 bake_state = "Constructing compact heightfield..."; // step #5
744
745 chf = rcAllocCompactHeightfield();
746
747 ERR_FAIL_COND(!chf);
748 ERR_FAIL_COND(!rcBuildCompactHeightfield(&ctx, cfg.walkableHeight, cfg.walkableClimb, *hf, *chf));
749
750 rcFreeHeightField(hf);
751 hf = nullptr;
752
753 bake_state = "Eroding walkable area..."; // step #6
754
755 ERR_FAIL_COND(!rcErodeWalkableArea(&ctx, cfg.walkableRadius, *chf));
756
757 bake_state = "Partitioning..."; // step #7
758
759 if (p_navigation_mesh->get_sample_partition_type() == NavigationMesh::SAMPLE_PARTITION_WATERSHED) {
760 ERR_FAIL_COND(!rcBuildDistanceField(&ctx, *chf));
761 ERR_FAIL_COND(!rcBuildRegions(&ctx, *chf, 0, cfg.minRegionArea, cfg.mergeRegionArea));
762 } else if (p_navigation_mesh->get_sample_partition_type() == NavigationMesh::SAMPLE_PARTITION_MONOTONE) {
763 ERR_FAIL_COND(!rcBuildRegionsMonotone(&ctx, *chf, 0, cfg.minRegionArea, cfg.mergeRegionArea));
764 } else {
765 ERR_FAIL_COND(!rcBuildLayerRegions(&ctx, *chf, 0, cfg.minRegionArea));
766 }
767
768 bake_state = "Creating contours..."; // step #8
769
770 cset = rcAllocContourSet();
771
772 ERR_FAIL_COND(!cset);
773 ERR_FAIL_COND(!rcBuildContours(&ctx, *chf, cfg.maxSimplificationError, cfg.maxEdgeLen, *cset));
774
775 bake_state = "Creating polymesh..."; // step #9
776
777 poly_mesh = rcAllocPolyMesh();
778 ERR_FAIL_COND(!poly_mesh);
779 ERR_FAIL_COND(!rcBuildPolyMesh(&ctx, *cset, cfg.maxVertsPerPoly, *poly_mesh));
780
781 detail_mesh = rcAllocPolyMeshDetail();
782 ERR_FAIL_COND(!detail_mesh);
783 ERR_FAIL_COND(!rcBuildPolyMeshDetail(&ctx, *poly_mesh, *chf, cfg.detailSampleDist, cfg.detailSampleMaxError, *detail_mesh));
784
785 rcFreeCompactHeightfield(chf);
786 chf = nullptr;
787 rcFreeContourSet(cset);
788 cset = nullptr;
789
790 bake_state = "Converting to native navigation mesh..."; // step #10
791
792 Vector<Vector3> nav_vertices;
793
794 for (int i = 0; i < detail_mesh->nverts; i++) {
795 const float *v = &detail_mesh->verts[i * 3];
796 nav_vertices.push_back(Vector3(v[0], v[1], v[2]));
797 }
798 p_navigation_mesh->set_vertices(nav_vertices);
799 p_navigation_mesh->clear_polygons();
800
801 for (int i = 0; i < detail_mesh->nmeshes; i++) {
802 const unsigned int *detail_mesh_m = &detail_mesh->meshes[i * 4];
803 const unsigned int detail_mesh_bverts = detail_mesh_m[0];
804 const unsigned int detail_mesh_m_btris = detail_mesh_m[2];
805 const unsigned int detail_mesh_ntris = detail_mesh_m[3];
806 const unsigned char *detail_mesh_tris = &detail_mesh->tris[detail_mesh_m_btris * 4];
807 for (unsigned int j = 0; j < detail_mesh_ntris; j++) {
808 Vector<int> nav_indices;
809 nav_indices.resize(3);
810 // Polygon order in recast is opposite than godot's
811 nav_indices.write[0] = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 0]));
812 nav_indices.write[1] = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 2]));
813 nav_indices.write[2] = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 1]));
814 p_navigation_mesh->add_polygon(nav_indices);
815 }
816 }
817
818 bake_state = "Cleanup..."; // step #11
819
820 rcFreePolyMesh(poly_mesh);
821 poly_mesh = nullptr;
822 rcFreePolyMeshDetail(detail_mesh);
823 detail_mesh = nullptr;
824
825 bake_state = "Baking finished."; // step #12
826}
827
828bool NavMeshGenerator3D::generator_emit_callback(const Callable &p_callback) {
829 ERR_FAIL_COND_V(!p_callback.is_valid(), false);
830
831 Callable::CallError ce;
832 Variant result;
833 p_callback.callp(nullptr, 0, result, ce);
834
835 return ce.error == Callable::CallError::CALL_OK;
836}
837
838#endif // _3D_DISABLED
839