1/**************************************************************************/
2/* nav_map.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#include "nav_map.h"
32
33#include "nav_agent.h"
34#include "nav_link.h"
35#include "nav_obstacle.h"
36#include "nav_region.h"
37
38#include "core/config/project_settings.h"
39#include "core/object/worker_thread_pool.h"
40
41#include <Obstacle2d.h>
42
43#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
44
45// Helper macro
46#define APPEND_METADATA(poly) \
47 if (r_path_types) { \
48 r_path_types->push_back(poly->owner->get_type()); \
49 } \
50 if (r_path_rids) { \
51 r_path_rids->push_back(poly->owner->get_self()); \
52 } \
53 if (r_path_owners) { \
54 r_path_owners->push_back(poly->owner->get_owner_id()); \
55 }
56
57void NavMap::set_up(Vector3 p_up) {
58 if (up == p_up) {
59 return;
60 }
61 up = p_up;
62 regenerate_polygons = true;
63}
64
65void NavMap::set_cell_size(real_t p_cell_size) {
66 if (cell_size == p_cell_size) {
67 return;
68 }
69 cell_size = p_cell_size;
70 regenerate_polygons = true;
71}
72
73void NavMap::set_cell_height(real_t p_cell_height) {
74 if (cell_height == p_cell_height) {
75 return;
76 }
77 cell_height = p_cell_height;
78 regenerate_polygons = true;
79}
80
81void NavMap::set_use_edge_connections(bool p_enabled) {
82 if (use_edge_connections == p_enabled) {
83 return;
84 }
85 use_edge_connections = p_enabled;
86 regenerate_links = true;
87}
88
89void NavMap::set_edge_connection_margin(real_t p_edge_connection_margin) {
90 if (edge_connection_margin == p_edge_connection_margin) {
91 return;
92 }
93 edge_connection_margin = p_edge_connection_margin;
94 regenerate_links = true;
95}
96
97void NavMap::set_link_connection_radius(real_t p_link_connection_radius) {
98 if (link_connection_radius == p_link_connection_radius) {
99 return;
100 }
101 link_connection_radius = p_link_connection_radius;
102 regenerate_links = true;
103}
104
105gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
106 const int x = static_cast<int>(Math::floor(p_pos.x / cell_size));
107 const int y = static_cast<int>(Math::floor(p_pos.y / cell_height));
108 const int z = static_cast<int>(Math::floor(p_pos.z / cell_size));
109
110 gd::PointKey p;
111 p.key = 0;
112 p.x = x;
113 p.y = y;
114 p.z = z;
115 return p;
116}
117
118Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const {
119 ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector<Vector3>(), "NavigationServer map query failed because it was made before first map synchronization.");
120 // Clear metadata outputs.
121 if (r_path_types) {
122 r_path_types->clear();
123 }
124 if (r_path_rids) {
125 r_path_rids->clear();
126 }
127 if (r_path_owners) {
128 r_path_owners->clear();
129 }
130
131 // Find the start poly and the end poly on this map.
132 const gd::Polygon *begin_poly = nullptr;
133 const gd::Polygon *end_poly = nullptr;
134 Vector3 begin_point;
135 Vector3 end_point;
136 real_t begin_d = FLT_MAX;
137 real_t end_d = FLT_MAX;
138 // Find the initial poly and the end poly on this map.
139 for (const gd::Polygon &p : polygons) {
140 // Only consider the polygon if it in a region with compatible layers.
141 if ((p_navigation_layers & p.owner->get_navigation_layers()) == 0) {
142 continue;
143 }
144
145 // For each face check the distance between the origin/destination
146 for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
147 const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
148
149 Vector3 point = face.get_closest_point_to(p_origin);
150 real_t distance_to_point = point.distance_to(p_origin);
151 if (distance_to_point < begin_d) {
152 begin_d = distance_to_point;
153 begin_poly = &p;
154 begin_point = point;
155 }
156
157 point = face.get_closest_point_to(p_destination);
158 distance_to_point = point.distance_to(p_destination);
159 if (distance_to_point < end_d) {
160 end_d = distance_to_point;
161 end_poly = &p;
162 end_point = point;
163 }
164 }
165 }
166
167 // Check for trivial cases
168 if (!begin_poly || !end_poly) {
169 return Vector<Vector3>();
170 }
171 if (begin_poly == end_poly) {
172 if (r_path_types) {
173 r_path_types->resize(2);
174 r_path_types->write[0] = begin_poly->owner->get_type();
175 r_path_types->write[1] = end_poly->owner->get_type();
176 }
177
178 if (r_path_rids) {
179 r_path_rids->resize(2);
180 (*r_path_rids)[0] = begin_poly->owner->get_self();
181 (*r_path_rids)[1] = end_poly->owner->get_self();
182 }
183
184 if (r_path_owners) {
185 r_path_owners->resize(2);
186 r_path_owners->write[0] = begin_poly->owner->get_owner_id();
187 r_path_owners->write[1] = end_poly->owner->get_owner_id();
188 }
189
190 Vector<Vector3> path;
191 path.resize(2);
192 path.write[0] = begin_point;
193 path.write[1] = end_point;
194 return path;
195 }
196
197 // List of all reachable navigation polys.
198 LocalVector<gd::NavigationPoly> navigation_polys;
199 navigation_polys.reserve(polygons.size() * 0.75);
200
201 // Add the start polygon to the reachable navigation polygons.
202 gd::NavigationPoly begin_navigation_poly = gd::NavigationPoly(begin_poly);
203 begin_navigation_poly.self_id = 0;
204 begin_navigation_poly.entry = begin_point;
205 begin_navigation_poly.back_navigation_edge_pathway_start = begin_point;
206 begin_navigation_poly.back_navigation_edge_pathway_end = begin_point;
207 navigation_polys.push_back(begin_navigation_poly);
208
209 // List of polygon IDs to visit.
210 List<uint32_t> to_visit;
211 to_visit.push_back(0);
212
213 // This is an implementation of the A* algorithm.
214 int least_cost_id = 0;
215 int prev_least_cost_id = -1;
216 bool found_route = false;
217
218 const gd::Polygon *reachable_end = nullptr;
219 real_t reachable_d = FLT_MAX;
220 bool is_reachable = true;
221
222 while (true) {
223 // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
224 for (const gd::Edge &edge : navigation_polys[least_cost_id].poly->edges) {
225 // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
226 for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
227 const gd::Edge::Connection &connection = edge.connections[connection_index];
228
229 // Only consider the connection to another polygon if this polygon is in a region with compatible layers.
230 if ((p_navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) {
231 continue;
232 }
233
234 const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id];
235 real_t poly_enter_cost = 0.0;
236 real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost();
237
238 if (prev_least_cost_id != -1 && (navigation_polys[prev_least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self())) {
239 poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost();
240 }
241 prev_least_cost_id = least_cost_id;
242
243 Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end };
244 const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway);
245 const real_t new_distance = (least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost) + poly_enter_cost + least_cost_poly.traveled_distance;
246
247 int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon));
248
249 if (already_visited_polygon_index != -1) {
250 // Polygon already visited, check if we can reduce the travel cost.
251 gd::NavigationPoly &avp = navigation_polys[already_visited_polygon_index];
252 if (new_distance < avp.traveled_distance) {
253 avp.back_navigation_poly_id = least_cost_id;
254 avp.back_navigation_edge = connection.edge;
255 avp.back_navigation_edge_pathway_start = connection.pathway_start;
256 avp.back_navigation_edge_pathway_end = connection.pathway_end;
257 avp.traveled_distance = new_distance;
258 avp.entry = new_entry;
259 }
260 } else {
261 // Add the neighbor polygon to the reachable ones.
262 gd::NavigationPoly new_navigation_poly = gd::NavigationPoly(connection.polygon);
263 new_navigation_poly.self_id = navigation_polys.size();
264 new_navigation_poly.back_navigation_poly_id = least_cost_id;
265 new_navigation_poly.back_navigation_edge = connection.edge;
266 new_navigation_poly.back_navigation_edge_pathway_start = connection.pathway_start;
267 new_navigation_poly.back_navigation_edge_pathway_end = connection.pathway_end;
268 new_navigation_poly.traveled_distance = new_distance;
269 new_navigation_poly.entry = new_entry;
270 navigation_polys.push_back(new_navigation_poly);
271
272 // Add the neighbor polygon to the polygons to visit.
273 to_visit.push_back(navigation_polys.size() - 1);
274 }
275 }
276 }
277
278 // Removes the least cost polygon from the list of polygons to visit so we can advance.
279 to_visit.erase(least_cost_id);
280
281 // When the list of polygons to visit is empty at this point it means the End Polygon is not reachable
282 if (to_visit.size() == 0) {
283 // Thus use the further reachable polygon
284 ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons");
285 is_reachable = false;
286 if (reachable_end == nullptr) {
287 // The path is not found and there is not a way out.
288 break;
289 }
290
291 // Set as end point the furthest reachable point.
292 end_poly = reachable_end;
293 end_d = FLT_MAX;
294 for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
295 Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
296 Vector3 spoint = f.get_closest_point_to(p_destination);
297 real_t dpoint = spoint.distance_to(p_destination);
298 if (dpoint < end_d) {
299 end_point = spoint;
300 end_d = dpoint;
301 }
302 }
303
304 // Search all faces of start polygon as well.
305 bool closest_point_on_start_poly = false;
306 for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
307 Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
308 Vector3 spoint = f.get_closest_point_to(p_destination);
309 real_t dpoint = spoint.distance_to(p_destination);
310 if (dpoint < end_d) {
311 end_point = spoint;
312 end_d = dpoint;
313 closest_point_on_start_poly = true;
314 }
315 }
316
317 if (closest_point_on_start_poly) {
318 // No point to run PostProcessing when start and end convex polygon is the same.
319 if (r_path_types) {
320 r_path_types->resize(2);
321 r_path_types->write[0] = begin_poly->owner->get_type();
322 r_path_types->write[1] = begin_poly->owner->get_type();
323 }
324
325 if (r_path_rids) {
326 r_path_rids->resize(2);
327 (*r_path_rids)[0] = begin_poly->owner->get_self();
328 (*r_path_rids)[1] = begin_poly->owner->get_self();
329 }
330
331 if (r_path_owners) {
332 r_path_owners->resize(2);
333 r_path_owners->write[0] = begin_poly->owner->get_owner_id();
334 r_path_owners->write[1] = begin_poly->owner->get_owner_id();
335 }
336
337 Vector<Vector3> path;
338 path.resize(2);
339 path.write[0] = begin_point;
340 path.write[1] = end_point;
341 return path;
342 }
343
344 // Reset open and navigation_polys
345 gd::NavigationPoly np = navigation_polys[0];
346 navigation_polys.clear();
347 navigation_polys.push_back(np);
348 to_visit.clear();
349 to_visit.push_back(0);
350 least_cost_id = 0;
351 prev_least_cost_id = -1;
352
353 reachable_end = nullptr;
354
355 continue;
356 }
357
358 // Find the polygon with the minimum cost from the list of polygons to visit.
359 least_cost_id = -1;
360 real_t least_cost = FLT_MAX;
361 for (List<uint32_t>::Element *element = to_visit.front(); element != nullptr; element = element->next()) {
362 gd::NavigationPoly *np = &navigation_polys[element->get()];
363 real_t cost = np->traveled_distance;
364 cost += (np->entry.distance_to(end_point) * np->poly->owner->get_travel_cost());
365 if (cost < least_cost) {
366 least_cost_id = np->self_id;
367 least_cost = cost;
368 }
369 }
370
371 ERR_BREAK(least_cost_id == -1);
372
373 // Stores the further reachable end polygon, in case our goal is not reachable.
374 if (is_reachable) {
375 real_t d = navigation_polys[least_cost_id].entry.distance_to(p_destination) * navigation_polys[least_cost_id].poly->owner->get_travel_cost();
376 if (reachable_d > d) {
377 reachable_d = d;
378 reachable_end = navigation_polys[least_cost_id].poly;
379 }
380 }
381
382 // Check if we reached the end
383 if (navigation_polys[least_cost_id].poly == end_poly) {
384 found_route = true;
385 break;
386 }
387 }
388
389 // We did not find a route but we have both a start polygon and an end polygon at this point.
390 // Usually this happens because there was not a single external or internal connected edge, e.g. our start polygon is an isolated, single convex polygon.
391 if (!found_route) {
392 end_d = FLT_MAX;
393 // Search all faces of the start polygon for the closest point to our target position.
394 for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
395 Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
396 Vector3 spoint = f.get_closest_point_to(p_destination);
397 real_t dpoint = spoint.distance_to(p_destination);
398 if (dpoint < end_d) {
399 end_point = spoint;
400 end_d = dpoint;
401 }
402 }
403
404 if (r_path_types) {
405 r_path_types->resize(2);
406 r_path_types->write[0] = begin_poly->owner->get_type();
407 r_path_types->write[1] = begin_poly->owner->get_type();
408 }
409
410 if (r_path_rids) {
411 r_path_rids->resize(2);
412 (*r_path_rids)[0] = begin_poly->owner->get_self();
413 (*r_path_rids)[1] = begin_poly->owner->get_self();
414 }
415
416 if (r_path_owners) {
417 r_path_owners->resize(2);
418 r_path_owners->write[0] = begin_poly->owner->get_owner_id();
419 r_path_owners->write[1] = begin_poly->owner->get_owner_id();
420 }
421
422 Vector<Vector3> path;
423 path.resize(2);
424 path.write[0] = begin_point;
425 path.write[1] = end_point;
426 return path;
427 }
428
429 Vector<Vector3> path;
430 // Optimize the path.
431 if (p_optimize) {
432 // Set the apex poly/point to the end point
433 gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
434
435 Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end };
436 const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_point, back_pathway);
437 if (end_point.is_equal_approx(back_edge_closest_point)) {
438 // The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing.
439 // At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon.
440 if (apex_poly->back_navigation_poly_id != -1) {
441 apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id];
442 }
443 }
444
445 Vector3 apex_point = end_point;
446
447 gd::NavigationPoly *left_poly = apex_poly;
448 Vector3 left_portal = apex_point;
449 gd::NavigationPoly *right_poly = apex_poly;
450 Vector3 right_portal = apex_point;
451
452 gd::NavigationPoly *p = apex_poly;
453
454 path.push_back(end_point);
455 APPEND_METADATA(end_poly);
456
457 while (p) {
458 // Set left and right points of the pathway between polygons.
459 Vector3 left = p->back_navigation_edge_pathway_start;
460 Vector3 right = p->back_navigation_edge_pathway_end;
461 if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(up) < 0) {
462 SWAP(left, right);
463 }
464
465 bool skip = false;
466 if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(up) >= 0) {
467 //process
468 if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(up) > 0) {
469 left_poly = p;
470 left_portal = left;
471 } else {
472 clip_path(navigation_polys, path, apex_poly, right_portal, right_poly, r_path_types, r_path_rids, r_path_owners);
473
474 apex_point = right_portal;
475 p = right_poly;
476 left_poly = p;
477 apex_poly = p;
478 left_portal = apex_point;
479 right_portal = apex_point;
480
481 path.push_back(apex_point);
482 APPEND_METADATA(apex_poly->poly);
483 skip = true;
484 }
485 }
486
487 if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(up) <= 0) {
488 //process
489 if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(up) < 0) {
490 right_poly = p;
491 right_portal = right;
492 } else {
493 clip_path(navigation_polys, path, apex_poly, left_portal, left_poly, r_path_types, r_path_rids, r_path_owners);
494
495 apex_point = left_portal;
496 p = left_poly;
497 right_poly = p;
498 apex_poly = p;
499 right_portal = apex_point;
500 left_portal = apex_point;
501
502 path.push_back(apex_point);
503 APPEND_METADATA(apex_poly->poly);
504 }
505 }
506
507 // Go to the previous polygon.
508 if (p->back_navigation_poly_id != -1) {
509 p = &navigation_polys[p->back_navigation_poly_id];
510 } else {
511 // The end
512 p = nullptr;
513 }
514 }
515
516 // If the last point is not the begin point, add it to the list.
517 if (path[path.size() - 1] != begin_point) {
518 path.push_back(begin_point);
519 APPEND_METADATA(begin_poly);
520 }
521
522 path.reverse();
523 if (r_path_types) {
524 r_path_types->reverse();
525 }
526 if (r_path_rids) {
527 r_path_rids->reverse();
528 }
529 if (r_path_owners) {
530 r_path_owners->reverse();
531 }
532
533 } else {
534 path.push_back(end_point);
535 APPEND_METADATA(end_poly);
536
537 // Add mid points
538 int np_id = least_cost_id;
539 while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
540 if (navigation_polys[np_id].back_navigation_edge != -1) {
541 int prev = navigation_polys[np_id].back_navigation_edge;
542 int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
543 Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
544
545 path.push_back(point);
546 APPEND_METADATA(navigation_polys[np_id].poly);
547 } else {
548 path.push_back(navigation_polys[np_id].entry);
549 APPEND_METADATA(navigation_polys[np_id].poly);
550 }
551
552 np_id = navigation_polys[np_id].back_navigation_poly_id;
553 }
554
555 path.push_back(begin_point);
556 APPEND_METADATA(begin_poly);
557
558 path.reverse();
559 if (r_path_types) {
560 r_path_types->reverse();
561 }
562 if (r_path_rids) {
563 r_path_rids->reverse();
564 }
565 if (r_path_owners) {
566 r_path_owners->reverse();
567 }
568 }
569
570 // Ensure post conditions (path arrays MUST match in size).
571 CRASH_COND(r_path_types && path.size() != r_path_types->size());
572 CRASH_COND(r_path_rids && path.size() != r_path_rids->size());
573 CRASH_COND(r_path_owners && path.size() != r_path_owners->size());
574
575 return path;
576}
577
578Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
579 ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
580 bool use_collision = p_use_collision;
581 Vector3 closest_point;
582 real_t closest_point_d = FLT_MAX;
583
584 for (const gd::Polygon &p : polygons) {
585 // For each face check the distance to the segment
586 for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
587 const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
588 Vector3 inters;
589 if (f.intersects_segment(p_from, p_to, &inters)) {
590 const real_t d = closest_point_d = p_from.distance_to(inters);
591 if (use_collision == false) {
592 closest_point = inters;
593 use_collision = true;
594 closest_point_d = d;
595 } else if (closest_point_d > d) {
596 closest_point = inters;
597 closest_point_d = d;
598 }
599 }
600 }
601
602 if (use_collision == false) {
603 for (size_t point_id = 0; point_id < p.points.size(); point_id += 1) {
604 Vector3 a, b;
605
606 Geometry3D::get_closest_points_between_segments(
607 p_from,
608 p_to,
609 p.points[point_id].pos,
610 p.points[(point_id + 1) % p.points.size()].pos,
611 a,
612 b);
613
614 const real_t d = a.distance_to(b);
615 if (d < closest_point_d) {
616 closest_point_d = d;
617 closest_point = b;
618 }
619 }
620 }
621 }
622
623 return closest_point;
624}
625
626Vector3 NavMap::get_closest_point(const Vector3 &p_point) const {
627 ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
628 gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
629 return cp.point;
630}
631
632Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const {
633 ERR_FAIL_COND_V_MSG(map_update_id == 0, Vector3(), "NavigationServer map query failed because it was made before first map synchronization.");
634 gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
635 return cp.normal;
636}
637
638RID NavMap::get_closest_point_owner(const Vector3 &p_point) const {
639 ERR_FAIL_COND_V_MSG(map_update_id == 0, RID(), "NavigationServer map query failed because it was made before first map synchronization.");
640 gd::ClosestPointQueryResult cp = get_closest_point_info(p_point);
641 return cp.owner;
642}
643
644gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const {
645 gd::ClosestPointQueryResult result;
646 real_t closest_point_ds = FLT_MAX;
647
648 for (const gd::Polygon &p : polygons) {
649 // For each face check the distance to the point
650 for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) {
651 const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
652 const Vector3 inters = f.get_closest_point_to(p_point);
653 const real_t ds = inters.distance_squared_to(p_point);
654 if (ds < closest_point_ds) {
655 result.point = inters;
656 result.normal = f.get_plane().normal;
657 result.owner = p.owner->get_self();
658 closest_point_ds = ds;
659 }
660 }
661 }
662
663 return result;
664}
665
666void NavMap::add_region(NavRegion *p_region) {
667 regions.push_back(p_region);
668 regenerate_links = true;
669}
670
671void NavMap::remove_region(NavRegion *p_region) {
672 int64_t region_index = regions.find(p_region);
673 if (region_index >= 0) {
674 regions.remove_at_unordered(region_index);
675 regenerate_links = true;
676 }
677}
678
679void NavMap::add_link(NavLink *p_link) {
680 links.push_back(p_link);
681 regenerate_links = true;
682}
683
684void NavMap::remove_link(NavLink *p_link) {
685 int64_t link_index = links.find(p_link);
686 if (link_index >= 0) {
687 links.remove_at_unordered(link_index);
688 regenerate_links = true;
689 }
690}
691
692bool NavMap::has_agent(NavAgent *agent) const {
693 return (agents.find(agent) >= 0);
694}
695
696void NavMap::add_agent(NavAgent *agent) {
697 if (!has_agent(agent)) {
698 agents.push_back(agent);
699 agents_dirty = true;
700 }
701}
702
703void NavMap::remove_agent(NavAgent *agent) {
704 remove_agent_as_controlled(agent);
705 int64_t agent_index = agents.find(agent);
706 if (agent_index >= 0) {
707 agents.remove_at_unordered(agent_index);
708 agents_dirty = true;
709 }
710}
711
712bool NavMap::has_obstacle(NavObstacle *obstacle) const {
713 return (obstacles.find(obstacle) >= 0);
714}
715
716void NavMap::add_obstacle(NavObstacle *obstacle) {
717 if (obstacle->get_paused()) {
718 // No point in adding a paused obstacle, it will add itself when unpaused again.
719 return;
720 }
721
722 if (!has_obstacle(obstacle)) {
723 obstacles.push_back(obstacle);
724 obstacles_dirty = true;
725 }
726}
727
728void NavMap::remove_obstacle(NavObstacle *obstacle) {
729 int64_t obstacle_index = obstacles.find(obstacle);
730 if (obstacle_index >= 0) {
731 obstacles.remove_at_unordered(obstacle_index);
732 obstacles_dirty = true;
733 }
734}
735
736void NavMap::set_agent_as_controlled(NavAgent *agent) {
737 remove_agent_as_controlled(agent);
738
739 if (agent->get_paused()) {
740 // No point in adding a paused agent, it will add itself when unpaused again.
741 return;
742 }
743
744 if (agent->get_use_3d_avoidance()) {
745 int64_t agent_3d_index = active_3d_avoidance_agents.find(agent);
746 if (agent_3d_index < 0) {
747 active_3d_avoidance_agents.push_back(agent);
748 agents_dirty = true;
749 }
750 } else {
751 int64_t agent_2d_index = active_2d_avoidance_agents.find(agent);
752 if (agent_2d_index < 0) {
753 active_2d_avoidance_agents.push_back(agent);
754 agents_dirty = true;
755 }
756 }
757}
758
759void NavMap::remove_agent_as_controlled(NavAgent *agent) {
760 int64_t agent_3d_index = active_3d_avoidance_agents.find(agent);
761 if (agent_3d_index >= 0) {
762 active_3d_avoidance_agents.remove_at_unordered(agent_3d_index);
763 agents_dirty = true;
764 }
765 int64_t agent_2d_index = active_2d_avoidance_agents.find(agent);
766 if (agent_2d_index >= 0) {
767 active_2d_avoidance_agents.remove_at_unordered(agent_2d_index);
768 agents_dirty = true;
769 }
770}
771
772void NavMap::sync() {
773 // Performance Monitor
774 int _new_pm_region_count = regions.size();
775 int _new_pm_agent_count = agents.size();
776 int _new_pm_link_count = links.size();
777 int _new_pm_polygon_count = pm_polygon_count;
778 int _new_pm_edge_count = pm_edge_count;
779 int _new_pm_edge_merge_count = pm_edge_merge_count;
780 int _new_pm_edge_connection_count = pm_edge_connection_count;
781 int _new_pm_edge_free_count = pm_edge_free_count;
782
783 // Check if we need to update the links.
784 if (regenerate_polygons) {
785 for (NavRegion *region : regions) {
786 region->scratch_polygons();
787 }
788 regenerate_links = true;
789 }
790
791 for (NavRegion *region : regions) {
792 if (region->sync()) {
793 regenerate_links = true;
794 }
795 }
796
797 for (NavLink *link : links) {
798 if (link->check_dirty()) {
799 regenerate_links = true;
800 }
801 }
802
803 if (regenerate_links) {
804 _new_pm_polygon_count = 0;
805 _new_pm_edge_count = 0;
806 _new_pm_edge_merge_count = 0;
807 _new_pm_edge_connection_count = 0;
808 _new_pm_edge_free_count = 0;
809
810 // Remove regions connections.
811 for (NavRegion *region : regions) {
812 region->get_connections().clear();
813 }
814
815 // Resize the polygon count.
816 int count = 0;
817 for (const NavRegion *region : regions) {
818 if (!region->get_enabled()) {
819 continue;
820 }
821 count += region->get_polygons().size();
822 }
823 polygons.resize(count);
824
825 // Copy all region polygons in the map.
826 count = 0;
827 for (const NavRegion *region : regions) {
828 if (!region->get_enabled()) {
829 continue;
830 }
831 const LocalVector<gd::Polygon> &polygons_source = region->get_polygons();
832 for (uint32_t n = 0; n < polygons_source.size(); n++) {
833 polygons[count + n] = polygons_source[n];
834 }
835 count += region->get_polygons().size();
836 }
837
838 _new_pm_polygon_count = polygons.size();
839
840 // Group all edges per key.
841 HashMap<gd::EdgeKey, Vector<gd::Edge::Connection>, gd::EdgeKey> connections;
842 for (gd::Polygon &poly : polygons) {
843 for (uint32_t p = 0; p < poly.points.size(); p++) {
844 int next_point = (p + 1) % poly.points.size();
845 gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
846
847 HashMap<gd::EdgeKey, Vector<gd::Edge::Connection>, gd::EdgeKey>::Iterator connection = connections.find(ek);
848 if (!connection) {
849 connections[ek] = Vector<gd::Edge::Connection>();
850 _new_pm_edge_count += 1;
851 }
852 if (connections[ek].size() <= 1) {
853 // Add the polygon/edge tuple to this key.
854 gd::Edge::Connection new_connection;
855 new_connection.polygon = &poly;
856 new_connection.edge = p;
857 new_connection.pathway_start = poly.points[p].pos;
858 new_connection.pathway_end = poly.points[next_point].pos;
859 connections[ek].push_back(new_connection);
860 } else {
861 // The edge is already connected with another edge, skip.
862 ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'.");
863 }
864 }
865 }
866
867 Vector<gd::Edge::Connection> free_edges;
868 for (KeyValue<gd::EdgeKey, Vector<gd::Edge::Connection>> &E : connections) {
869 if (E.value.size() == 2) {
870 // Connect edge that are shared in different polygons.
871 gd::Edge::Connection &c1 = E.value.write[0];
872 gd::Edge::Connection &c2 = E.value.write[1];
873 c1.polygon->edges[c1.edge].connections.push_back(c2);
874 c2.polygon->edges[c2.edge].connections.push_back(c1);
875 // Note: The pathway_start/end are full for those connection and do not need to be modified.
876 _new_pm_edge_merge_count += 1;
877 } else {
878 CRASH_COND_MSG(E.value.size() != 1, vformat("Number of connection != 1. Found: %d", E.value.size()));
879 if (use_edge_connections && E.value[0].polygon->owner->get_use_edge_connections()) {
880 free_edges.push_back(E.value[0]);
881 }
882 }
883 }
884
885 // Find the compatible near edges.
886 //
887 // Note:
888 // Considering that the edges must be compatible (for obvious reasons)
889 // to be connected, create new polygons to remove that small gap is
890 // not really useful and would result in wasteful computation during
891 // connection, integration and path finding.
892 _new_pm_edge_free_count = free_edges.size();
893
894 for (int i = 0; i < free_edges.size(); i++) {
895 const gd::Edge::Connection &free_edge = free_edges[i];
896 Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
897 Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos;
898
899 for (int j = 0; j < free_edges.size(); j++) {
900 const gd::Edge::Connection &other_edge = free_edges[j];
901 if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
902 continue;
903 }
904
905 Vector3 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos;
906 Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
907
908 // Compute the projection of the opposite edge on the current one
909 Vector3 edge_vector = edge_p2 - edge_p1;
910 real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector.length_squared());
911 real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector.length_squared());
912 if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
913 continue;
914 }
915
916 // Check if the two edges are close to each other enough and compute a pathway between the two regions.
917 Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
918 Vector3 other1;
919 if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) {
920 other1 = other_edge_p1;
921 } else {
922 other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
923 }
924 if (other1.distance_to(self1) > edge_connection_margin) {
925 continue;
926 }
927
928 Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
929 Vector3 other2;
930 if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) {
931 other2 = other_edge_p2;
932 } else {
933 other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
934 }
935 if (other2.distance_to(self2) > edge_connection_margin) {
936 continue;
937 }
938
939 // The edges can now be connected.
940 gd::Edge::Connection new_connection = other_edge;
941 new_connection.pathway_start = (self1 + other1) / 2.0;
942 new_connection.pathway_end = (self2 + other2) / 2.0;
943 free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
944
945 // Add the connection to the region_connection map.
946 ((NavRegion *)free_edge.polygon->owner)->get_connections().push_back(new_connection);
947 _new_pm_edge_connection_count += 1;
948 }
949 }
950
951 uint32_t link_poly_idx = 0;
952 link_polygons.resize(links.size());
953
954 // Search for polygons within range of a nav link.
955 for (const NavLink *link : links) {
956 const Vector3 start = link->get_start_position();
957 const Vector3 end = link->get_end_position();
958
959 gd::Polygon *closest_start_polygon = nullptr;
960 real_t closest_start_distance = link_connection_radius;
961 Vector3 closest_start_point;
962
963 gd::Polygon *closest_end_polygon = nullptr;
964 real_t closest_end_distance = link_connection_radius;
965 Vector3 closest_end_point;
966
967 // Create link to any polygons within the search radius of the start point.
968 for (uint32_t start_index = 0; start_index < polygons.size(); start_index++) {
969 gd::Polygon &start_poly = polygons[start_index];
970
971 // For each face check the distance to the start
972 for (uint32_t start_point_id = 2; start_point_id < start_poly.points.size(); start_point_id += 1) {
973 const Face3 start_face(start_poly.points[0].pos, start_poly.points[start_point_id - 1].pos, start_poly.points[start_point_id].pos);
974 const Vector3 start_point = start_face.get_closest_point_to(start);
975 const real_t start_distance = start_point.distance_to(start);
976
977 // Pick the polygon that is within our radius and is closer than anything we've seen yet.
978 if (start_distance <= link_connection_radius && start_distance < closest_start_distance) {
979 closest_start_distance = start_distance;
980 closest_start_point = start_point;
981 closest_start_polygon = &start_poly;
982 }
983 }
984 }
985
986 // Find any polygons within the search radius of the end point.
987 for (gd::Polygon &end_poly : polygons) {
988 // For each face check the distance to the end
989 for (uint32_t end_point_id = 2; end_point_id < end_poly.points.size(); end_point_id += 1) {
990 const Face3 end_face(end_poly.points[0].pos, end_poly.points[end_point_id - 1].pos, end_poly.points[end_point_id].pos);
991 const Vector3 end_point = end_face.get_closest_point_to(end);
992 const real_t end_distance = end_point.distance_to(end);
993
994 // Pick the polygon that is within our radius and is closer than anything we've seen yet.
995 if (end_distance <= link_connection_radius && end_distance < closest_end_distance) {
996 closest_end_distance = end_distance;
997 closest_end_point = end_point;
998 closest_end_polygon = &end_poly;
999 }
1000 }
1001 }
1002
1003 // If we have both a start and end point, then create a synthetic polygon to route through.
1004 if (closest_start_polygon && closest_end_polygon) {
1005 gd::Polygon &new_polygon = link_polygons[link_poly_idx++];
1006 new_polygon.owner = link;
1007
1008 new_polygon.edges.clear();
1009 new_polygon.edges.resize(4);
1010 new_polygon.points.clear();
1011 new_polygon.points.reserve(4);
1012
1013 // Build a set of vertices that create a thin polygon going from the start to the end point.
1014 new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) });
1015 new_polygon.points.push_back({ closest_start_point, get_point_key(closest_start_point) });
1016 new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) });
1017 new_polygon.points.push_back({ closest_end_point, get_point_key(closest_end_point) });
1018
1019 Vector3 center;
1020 for (int p = 0; p < 4; ++p) {
1021 center += new_polygon.points[p].pos;
1022 }
1023 new_polygon.center = center / real_t(new_polygon.points.size());
1024 new_polygon.clockwise = true;
1025
1026 // Setup connections to go forward in the link.
1027 {
1028 gd::Edge::Connection entry_connection;
1029 entry_connection.polygon = &new_polygon;
1030 entry_connection.edge = -1;
1031 entry_connection.pathway_start = new_polygon.points[0].pos;
1032 entry_connection.pathway_end = new_polygon.points[1].pos;
1033 closest_start_polygon->edges[0].connections.push_back(entry_connection);
1034
1035 gd::Edge::Connection exit_connection;
1036 exit_connection.polygon = closest_end_polygon;
1037 exit_connection.edge = -1;
1038 exit_connection.pathway_start = new_polygon.points[2].pos;
1039 exit_connection.pathway_end = new_polygon.points[3].pos;
1040 new_polygon.edges[2].connections.push_back(exit_connection);
1041 }
1042
1043 // If the link is bi-directional, create connections from the end to the start.
1044 if (link->is_bidirectional()) {
1045 gd::Edge::Connection entry_connection;
1046 entry_connection.polygon = &new_polygon;
1047 entry_connection.edge = -1;
1048 entry_connection.pathway_start = new_polygon.points[2].pos;
1049 entry_connection.pathway_end = new_polygon.points[3].pos;
1050 closest_end_polygon->edges[0].connections.push_back(entry_connection);
1051
1052 gd::Edge::Connection exit_connection;
1053 exit_connection.polygon = closest_start_polygon;
1054 exit_connection.edge = -1;
1055 exit_connection.pathway_start = new_polygon.points[0].pos;
1056 exit_connection.pathway_end = new_polygon.points[1].pos;
1057 new_polygon.edges[0].connections.push_back(exit_connection);
1058 }
1059 }
1060 }
1061
1062 // Update the update ID.
1063 // Some code treats 0 as a failure case, so we avoid returning 0.
1064 map_update_id = map_update_id % 9999999 + 1;
1065 }
1066
1067 // Do we have modified obstacle positions?
1068 for (NavObstacle *obstacle : obstacles) {
1069 if (obstacle->check_dirty()) {
1070 obstacles_dirty = true;
1071 }
1072 }
1073 // Do we have modified agent arrays?
1074 for (NavAgent *agent : agents) {
1075 if (agent->check_dirty()) {
1076 agents_dirty = true;
1077 }
1078 }
1079
1080 // Update avoidance worlds.
1081 if (obstacles_dirty || agents_dirty) {
1082 _update_rvo_simulation();
1083 }
1084
1085 regenerate_polygons = false;
1086 regenerate_links = false;
1087 obstacles_dirty = false;
1088 agents_dirty = false;
1089
1090 // Performance Monitor.
1091 pm_region_count = _new_pm_region_count;
1092 pm_agent_count = _new_pm_agent_count;
1093 pm_link_count = _new_pm_link_count;
1094 pm_polygon_count = _new_pm_polygon_count;
1095 pm_edge_count = _new_pm_edge_count;
1096 pm_edge_merge_count = _new_pm_edge_merge_count;
1097 pm_edge_connection_count = _new_pm_edge_connection_count;
1098 pm_edge_free_count = _new_pm_edge_free_count;
1099}
1100
1101void NavMap::_update_rvo_obstacles_tree_2d() {
1102 int obstacle_vertex_count = 0;
1103 for (NavObstacle *obstacle : obstacles) {
1104 obstacle_vertex_count += obstacle->get_vertices().size();
1105 }
1106
1107 // Cannot use LocalVector here as RVO library expects std::vector to build KdTree
1108 std::vector<RVO2D::Obstacle2D *> raw_obstacles;
1109 raw_obstacles.reserve(obstacle_vertex_count);
1110
1111 // The following block is modified copy from RVO2D::AddObstacle()
1112 // Obstacles are linked and depend on all other obstacles.
1113 for (NavObstacle *obstacle : obstacles) {
1114 const Vector3 &_obstacle_position = obstacle->get_position();
1115 const Vector<Vector3> &_obstacle_vertices = obstacle->get_vertices();
1116
1117 if (_obstacle_vertices.size() < 2) {
1118 continue;
1119 }
1120
1121 std::vector<RVO2D::Vector2> rvo_2d_vertices;
1122 rvo_2d_vertices.reserve(_obstacle_vertices.size());
1123
1124 uint32_t _obstacle_avoidance_layers = obstacle->get_avoidance_layers();
1125
1126 for (const Vector3 &_obstacle_vertex : _obstacle_vertices) {
1127 rvo_2d_vertices.push_back(RVO2D::Vector2(_obstacle_vertex.x + _obstacle_position.x, _obstacle_vertex.z + _obstacle_position.z));
1128 }
1129
1130 const size_t obstacleNo = raw_obstacles.size();
1131
1132 for (size_t i = 0; i < rvo_2d_vertices.size(); i++) {
1133 RVO2D::Obstacle2D *rvo_2d_obstacle = new RVO2D::Obstacle2D();
1134 rvo_2d_obstacle->point_ = rvo_2d_vertices[i];
1135 rvo_2d_obstacle->avoidance_layers_ = _obstacle_avoidance_layers;
1136
1137 if (i != 0) {
1138 rvo_2d_obstacle->prevObstacle_ = raw_obstacles.back();
1139 rvo_2d_obstacle->prevObstacle_->nextObstacle_ = rvo_2d_obstacle;
1140 }
1141
1142 if (i == rvo_2d_vertices.size() - 1) {
1143 rvo_2d_obstacle->nextObstacle_ = raw_obstacles[obstacleNo];
1144 rvo_2d_obstacle->nextObstacle_->prevObstacle_ = rvo_2d_obstacle;
1145 }
1146
1147 rvo_2d_obstacle->unitDir_ = normalize(rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)] - rvo_2d_vertices[i]);
1148
1149 if (rvo_2d_vertices.size() == 2) {
1150 rvo_2d_obstacle->isConvex_ = true;
1151 } else {
1152 rvo_2d_obstacle->isConvex_ = (leftOf(rvo_2d_vertices[(i == 0 ? rvo_2d_vertices.size() - 1 : i - 1)], rvo_2d_vertices[i], rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f);
1153 }
1154
1155 rvo_2d_obstacle->id_ = raw_obstacles.size();
1156
1157 raw_obstacles.push_back(rvo_2d_obstacle);
1158 }
1159 }
1160
1161 rvo_simulation_2d.kdTree_->buildObstacleTree(raw_obstacles);
1162}
1163
1164void NavMap::_update_rvo_agents_tree_2d() {
1165 // Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
1166 std::vector<RVO2D::Agent2D *> raw_agents;
1167 raw_agents.reserve(active_2d_avoidance_agents.size());
1168 for (NavAgent *agent : active_2d_avoidance_agents) {
1169 raw_agents.push_back(agent->get_rvo_agent_2d());
1170 }
1171 rvo_simulation_2d.kdTree_->buildAgentTree(raw_agents);
1172}
1173
1174void NavMap::_update_rvo_agents_tree_3d() {
1175 // Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
1176 std::vector<RVO3D::Agent3D *> raw_agents;
1177 raw_agents.reserve(active_3d_avoidance_agents.size());
1178 for (NavAgent *agent : active_3d_avoidance_agents) {
1179 raw_agents.push_back(agent->get_rvo_agent_3d());
1180 }
1181 rvo_simulation_3d.kdTree_->buildAgentTree(raw_agents);
1182}
1183
1184void NavMap::_update_rvo_simulation() {
1185 if (obstacles_dirty) {
1186 _update_rvo_obstacles_tree_2d();
1187 }
1188 if (agents_dirty) {
1189 _update_rvo_agents_tree_2d();
1190 _update_rvo_agents_tree_3d();
1191 }
1192}
1193
1194void NavMap::compute_single_avoidance_step_2d(uint32_t index, NavAgent **agent) {
1195 (*(agent + index))->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
1196 (*(agent + index))->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
1197 (*(agent + index))->get_rvo_agent_2d()->update(&rvo_simulation_2d);
1198 (*(agent + index))->update();
1199}
1200
1201void NavMap::compute_single_avoidance_step_3d(uint32_t index, NavAgent **agent) {
1202 (*(agent + index))->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d);
1203 (*(agent + index))->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d);
1204 (*(agent + index))->get_rvo_agent_3d()->update(&rvo_simulation_3d);
1205 (*(agent + index))->update();
1206}
1207
1208void NavMap::step(real_t p_deltatime) {
1209 deltatime = p_deltatime;
1210
1211 rvo_simulation_2d.setTimeStep(float(deltatime));
1212 rvo_simulation_3d.setTimeStep(float(deltatime));
1213
1214 if (active_2d_avoidance_agents.size() > 0) {
1215 if (use_threads && avoidance_use_multiple_threads) {
1216 WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_avoidance_step_2d, active_2d_avoidance_agents.ptr(), active_2d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents2D"));
1217 WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
1218 } else {
1219 for (NavAgent *agent : active_2d_avoidance_agents) {
1220 agent->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
1221 agent->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
1222 agent->get_rvo_agent_2d()->update(&rvo_simulation_2d);
1223 agent->update();
1224 }
1225 }
1226 }
1227
1228 if (active_3d_avoidance_agents.size() > 0) {
1229 if (use_threads && avoidance_use_multiple_threads) {
1230 WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap::compute_single_avoidance_step_3d, active_3d_avoidance_agents.ptr(), active_3d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents3D"));
1231 WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
1232 } else {
1233 for (NavAgent *agent : active_3d_avoidance_agents) {
1234 agent->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d);
1235 agent->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d);
1236 agent->get_rvo_agent_3d()->update(&rvo_simulation_3d);
1237 agent->update();
1238 }
1239 }
1240 }
1241}
1242
1243void NavMap::dispatch_callbacks() {
1244 for (NavAgent *agent : active_2d_avoidance_agents) {
1245 agent->dispatch_avoidance_callback();
1246 }
1247
1248 for (NavAgent *agent : active_3d_avoidance_agents) {
1249 agent->dispatch_avoidance_callback();
1250 }
1251}
1252
1253void NavMap::clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const {
1254 Vector3 from = path[path.size() - 1];
1255
1256 if (from.is_equal_approx(p_to_point)) {
1257 return;
1258 }
1259 Plane cut_plane;
1260 cut_plane.normal = (from - p_to_point).cross(up);
1261 if (cut_plane.normal == Vector3()) {
1262 return;
1263 }
1264 cut_plane.normal.normalize();
1265 cut_plane.d = cut_plane.normal.dot(from);
1266
1267 while (from_poly != p_to_poly) {
1268 Vector3 pathway_start = from_poly->back_navigation_edge_pathway_start;
1269 Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end;
1270
1271 ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1);
1272 from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id];
1273
1274 if (!pathway_start.is_equal_approx(pathway_end)) {
1275 Vector3 inters;
1276 if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) {
1277 if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(path[path.size() - 1])) {
1278 path.push_back(inters);
1279 APPEND_METADATA(from_poly->poly);
1280 }
1281 }
1282 }
1283 }
1284}
1285
1286NavMap::NavMap() {
1287 avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads");
1288 avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads");
1289}
1290
1291NavMap::~NavMap() {
1292}
1293