1/**************************************************************************/
2/* a_star.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 "a_star.h"
32
33#include "core/math/geometry_3d.h"
34#include "core/object/script_language.h"
35
36int64_t AStar3D::get_available_point_id() const {
37 if (points.has(last_free_id)) {
38 int64_t cur_new_id = last_free_id + 1;
39 while (points.has(cur_new_id)) {
40 cur_new_id++;
41 }
42 const_cast<int64_t &>(last_free_id) = cur_new_id;
43 }
44
45 return last_free_id;
46}
47
48void AStar3D::add_point(int64_t p_id, const Vector3 &p_pos, real_t p_weight_scale) {
49 ERR_FAIL_COND_MSG(p_id < 0, vformat("Can't add a point with negative id: %d.", p_id));
50 ERR_FAIL_COND_MSG(p_weight_scale < 0.0, vformat("Can't add a point with weight scale less than 0.0: %f.", p_weight_scale));
51
52 Point *found_pt;
53 bool p_exists = points.lookup(p_id, found_pt);
54
55 if (!p_exists) {
56 Point *pt = memnew(Point);
57 pt->id = p_id;
58 pt->pos = p_pos;
59 pt->weight_scale = p_weight_scale;
60 pt->prev_point = nullptr;
61 pt->open_pass = 0;
62 pt->closed_pass = 0;
63 pt->enabled = true;
64 points.set(p_id, pt);
65 } else {
66 found_pt->pos = p_pos;
67 found_pt->weight_scale = p_weight_scale;
68 }
69}
70
71Vector3 AStar3D::get_point_position(int64_t p_id) const {
72 Point *p;
73 bool p_exists = points.lookup(p_id, p);
74 ERR_FAIL_COND_V_MSG(!p_exists, Vector3(), vformat("Can't get point's position. Point with id: %d doesn't exist.", p_id));
75
76 return p->pos;
77}
78
79void AStar3D::set_point_position(int64_t p_id, const Vector3 &p_pos) {
80 Point *p;
81 bool p_exists = points.lookup(p_id, p);
82 ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set point's position. Point with id: %d doesn't exist.", p_id));
83
84 p->pos = p_pos;
85}
86
87real_t AStar3D::get_point_weight_scale(int64_t p_id) const {
88 Point *p;
89 bool p_exists = points.lookup(p_id, p);
90 ERR_FAIL_COND_V_MSG(!p_exists, 0, vformat("Can't get point's weight scale. Point with id: %d doesn't exist.", p_id));
91
92 return p->weight_scale;
93}
94
95void AStar3D::set_point_weight_scale(int64_t p_id, real_t p_weight_scale) {
96 Point *p;
97 bool p_exists = points.lookup(p_id, p);
98 ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set point's weight scale. Point with id: %d doesn't exist.", p_id));
99 ERR_FAIL_COND_MSG(p_weight_scale < 0.0, vformat("Can't set point's weight scale less than 0.0: %f.", p_weight_scale));
100
101 p->weight_scale = p_weight_scale;
102}
103
104void AStar3D::remove_point(int64_t p_id) {
105 Point *p;
106 bool p_exists = points.lookup(p_id, p);
107 ERR_FAIL_COND_MSG(!p_exists, vformat("Can't remove point. Point with id: %d doesn't exist.", p_id));
108
109 for (OAHashMap<int64_t, Point *>::Iterator it = p->neighbors.iter(); it.valid; it = p->neighbors.next_iter(it)) {
110 Segment s(p_id, (*it.key));
111 segments.erase(s);
112
113 (*it.value)->neighbors.remove(p->id);
114 (*it.value)->unlinked_neighbours.remove(p->id);
115 }
116
117 for (OAHashMap<int64_t, Point *>::Iterator it = p->unlinked_neighbours.iter(); it.valid; it = p->unlinked_neighbours.next_iter(it)) {
118 Segment s(p_id, (*it.key));
119 segments.erase(s);
120
121 (*it.value)->neighbors.remove(p->id);
122 (*it.value)->unlinked_neighbours.remove(p->id);
123 }
124
125 memdelete(p);
126 points.remove(p_id);
127 last_free_id = p_id;
128}
129
130void AStar3D::connect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) {
131 ERR_FAIL_COND_MSG(p_id == p_with_id, vformat("Can't connect point with id: %d to itself.", p_id));
132
133 Point *a;
134 bool from_exists = points.lookup(p_id, a);
135 ERR_FAIL_COND_MSG(!from_exists, vformat("Can't connect points. Point with id: %d doesn't exist.", p_id));
136
137 Point *b;
138 bool to_exists = points.lookup(p_with_id, b);
139 ERR_FAIL_COND_MSG(!to_exists, vformat("Can't connect points. Point with id: %d doesn't exist.", p_with_id));
140
141 a->neighbors.set(b->id, b);
142
143 if (bidirectional) {
144 b->neighbors.set(a->id, a);
145 } else {
146 b->unlinked_neighbours.set(a->id, a);
147 }
148
149 Segment s(p_id, p_with_id);
150 if (bidirectional) {
151 s.direction = Segment::BIDIRECTIONAL;
152 }
153
154 HashSet<Segment, Segment>::Iterator element = segments.find(s);
155 if (element) {
156 s.direction |= element->direction;
157 if (s.direction == Segment::BIDIRECTIONAL) {
158 // Both are neighbors of each other now
159 a->unlinked_neighbours.remove(b->id);
160 b->unlinked_neighbours.remove(a->id);
161 }
162 segments.remove(element);
163 }
164
165 segments.insert(s);
166}
167
168void AStar3D::disconnect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) {
169 Point *a;
170 bool a_exists = points.lookup(p_id, a);
171 ERR_FAIL_COND_MSG(!a_exists, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_id));
172
173 Point *b;
174 bool b_exists = points.lookup(p_with_id, b);
175 ERR_FAIL_COND_MSG(!b_exists, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_with_id));
176
177 Segment s(p_id, p_with_id);
178 int remove_direction = bidirectional ? (int)Segment::BIDIRECTIONAL : (int)s.direction;
179
180 HashSet<Segment, Segment>::Iterator element = segments.find(s);
181 if (element) {
182 // s is the new segment
183 // Erase the directions to be removed
184 s.direction = (element->direction & ~remove_direction);
185
186 a->neighbors.remove(b->id);
187 if (bidirectional) {
188 b->neighbors.remove(a->id);
189 if (element->direction != Segment::BIDIRECTIONAL) {
190 a->unlinked_neighbours.remove(b->id);
191 b->unlinked_neighbours.remove(a->id);
192 }
193 } else {
194 if (s.direction == Segment::NONE) {
195 b->unlinked_neighbours.remove(a->id);
196 } else {
197 a->unlinked_neighbours.set(b->id, b);
198 }
199 }
200
201 segments.remove(element);
202 if (s.direction != Segment::NONE) {
203 segments.insert(s);
204 }
205 }
206}
207
208bool AStar3D::has_point(int64_t p_id) const {
209 return points.has(p_id);
210}
211
212PackedInt64Array AStar3D::get_point_ids() {
213 PackedInt64Array point_list;
214
215 for (OAHashMap<int64_t, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) {
216 point_list.push_back(*(it.key));
217 }
218
219 return point_list;
220}
221
222Vector<int64_t> AStar3D::get_point_connections(int64_t p_id) {
223 Point *p;
224 bool p_exists = points.lookup(p_id, p);
225 ERR_FAIL_COND_V_MSG(!p_exists, Vector<int64_t>(), vformat("Can't get point's connections. Point with id: %d doesn't exist.", p_id));
226
227 Vector<int64_t> point_list;
228
229 for (OAHashMap<int64_t, Point *>::Iterator it = p->neighbors.iter(); it.valid; it = p->neighbors.next_iter(it)) {
230 point_list.push_back((*it.key));
231 }
232
233 return point_list;
234}
235
236bool AStar3D::are_points_connected(int64_t p_id, int64_t p_with_id, bool bidirectional) const {
237 Segment s(p_id, p_with_id);
238 const HashSet<Segment, Segment>::Iterator element = segments.find(s);
239
240 return element &&
241 (bidirectional || (element->direction & s.direction) == s.direction);
242}
243
244void AStar3D::clear() {
245 last_free_id = 0;
246 for (OAHashMap<int64_t, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) {
247 memdelete(*(it.value));
248 }
249 segments.clear();
250 points.clear();
251}
252
253int64_t AStar3D::get_point_count() const {
254 return points.get_num_elements();
255}
256
257int64_t AStar3D::get_point_capacity() const {
258 return points.get_capacity();
259}
260
261void AStar3D::reserve_space(int64_t p_num_nodes) {
262 ERR_FAIL_COND_MSG(p_num_nodes <= 0, vformat("New capacity must be greater than 0, new was: %d.", p_num_nodes));
263 ERR_FAIL_COND_MSG((uint32_t)p_num_nodes < points.get_capacity(), vformat("New capacity must be greater than current capacity: %d, new was: %d.", points.get_capacity(), p_num_nodes));
264 points.reserve(p_num_nodes);
265}
266
267int64_t AStar3D::get_closest_point(const Vector3 &p_point, bool p_include_disabled) const {
268 int64_t closest_id = -1;
269 real_t closest_dist = 1e20;
270
271 for (OAHashMap<int64_t, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) {
272 if (!p_include_disabled && !(*it.value)->enabled) {
273 continue; // Disabled points should not be considered.
274 }
275
276 // Keep the closest point's ID, and in case of multiple closest IDs,
277 // the smallest one (makes it deterministic).
278 real_t d = p_point.distance_squared_to((*it.value)->pos);
279 int64_t id = *(it.key);
280 if (d <= closest_dist) {
281 if (d == closest_dist && id > closest_id) { // Keep lowest ID.
282 continue;
283 }
284 closest_dist = d;
285 closest_id = id;
286 }
287 }
288
289 return closest_id;
290}
291
292Vector3 AStar3D::get_closest_position_in_segment(const Vector3 &p_point) const {
293 real_t closest_dist = 1e20;
294 Vector3 closest_point;
295
296 for (const Segment &E : segments) {
297 Point *from_point = nullptr, *to_point = nullptr;
298 points.lookup(E.key.first, from_point);
299 points.lookup(E.key.second, to_point);
300
301 if (!(from_point->enabled && to_point->enabled)) {
302 continue;
303 }
304
305 Vector3 segment[2] = {
306 from_point->pos,
307 to_point->pos,
308 };
309
310 Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, segment);
311 real_t d = p_point.distance_squared_to(p);
312 if (d < closest_dist) {
313 closest_point = p;
314 closest_dist = d;
315 }
316 }
317
318 return closest_point;
319}
320
321bool AStar3D::_solve(Point *begin_point, Point *end_point) {
322 pass++;
323
324 if (!end_point->enabled) {
325 return false;
326 }
327
328 bool found_route = false;
329
330 LocalVector<Point *> open_list;
331 SortArray<Point *, SortPoints> sorter;
332
333 begin_point->g_score = 0;
334 begin_point->f_score = _estimate_cost(begin_point->id, end_point->id);
335 open_list.push_back(begin_point);
336
337 while (!open_list.is_empty()) {
338 Point *p = open_list[0]; // The currently processed point.
339
340 if (p == end_point) {
341 found_route = true;
342 break;
343 }
344
345 sorter.pop_heap(0, open_list.size(), open_list.ptr()); // Remove the current point from the open list.
346 open_list.remove_at(open_list.size() - 1);
347 p->closed_pass = pass; // Mark the point as closed.
348
349 for (OAHashMap<int64_t, Point *>::Iterator it = p->neighbors.iter(); it.valid; it = p->neighbors.next_iter(it)) {
350 Point *e = *(it.value); // The neighbor point.
351
352 if (!e->enabled || e->closed_pass == pass) {
353 continue;
354 }
355
356 real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * e->weight_scale;
357
358 bool new_point = false;
359
360 if (e->open_pass != pass) { // The point wasn't inside the open list.
361 e->open_pass = pass;
362 open_list.push_back(e);
363 new_point = true;
364 } else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous.
365 continue;
366 }
367
368 e->prev_point = p;
369 e->g_score = tentative_g_score;
370 e->f_score = e->g_score + _estimate_cost(e->id, end_point->id);
371
372 if (new_point) { // The position of the new points is already known.
373 sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptr());
374 } else {
375 sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptr());
376 }
377 }
378 }
379
380 return found_route;
381}
382
383real_t AStar3D::_estimate_cost(int64_t p_from_id, int64_t p_to_id) {
384 real_t scost;
385 if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_to_id, scost)) {
386 return scost;
387 }
388
389 Point *from_point;
390 bool from_exists = points.lookup(p_from_id, from_point);
391 ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
392
393 Point *to_point;
394 bool to_exists = points.lookup(p_to_id, to_point);
395 ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_to_id));
396
397 return from_point->pos.distance_to(to_point->pos);
398}
399
400real_t AStar3D::_compute_cost(int64_t p_from_id, int64_t p_to_id) {
401 real_t scost;
402 if (GDVIRTUAL_CALL(_compute_cost, p_from_id, p_to_id, scost)) {
403 return scost;
404 }
405
406 Point *from_point;
407 bool from_exists = points.lookup(p_from_id, from_point);
408 ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_from_id));
409
410 Point *to_point;
411 bool to_exists = points.lookup(p_to_id, to_point);
412 ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_to_id));
413
414 return from_point->pos.distance_to(to_point->pos);
415}
416
417Vector<Vector3> AStar3D::get_point_path(int64_t p_from_id, int64_t p_to_id) {
418 Point *a;
419 bool from_exists = points.lookup(p_from_id, a);
420 ERR_FAIL_COND_V_MSG(!from_exists, Vector<Vector3>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_from_id));
421
422 Point *b;
423 bool to_exists = points.lookup(p_to_id, b);
424 ERR_FAIL_COND_V_MSG(!to_exists, Vector<Vector3>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_to_id));
425
426 if (a == b) {
427 Vector<Vector3> ret;
428 ret.push_back(a->pos);
429 return ret;
430 }
431
432 Point *begin_point = a;
433 Point *end_point = b;
434
435 bool found_route = _solve(begin_point, end_point);
436 if (!found_route) {
437 return Vector<Vector3>();
438 }
439
440 Point *p = end_point;
441 int64_t pc = 1; // Begin point
442 while (p != begin_point) {
443 pc++;
444 p = p->prev_point;
445 }
446
447 Vector<Vector3> path;
448 path.resize(pc);
449
450 {
451 Vector3 *w = path.ptrw();
452
453 Point *p2 = end_point;
454 int64_t idx = pc - 1;
455 while (p2 != begin_point) {
456 w[idx--] = p2->pos;
457 p2 = p2->prev_point;
458 }
459
460 w[0] = p2->pos; // Assign first
461 }
462
463 return path;
464}
465
466Vector<int64_t> AStar3D::get_id_path(int64_t p_from_id, int64_t p_to_id) {
467 Point *a;
468 bool from_exists = points.lookup(p_from_id, a);
469 ERR_FAIL_COND_V_MSG(!from_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id));
470
471 Point *b;
472 bool to_exists = points.lookup(p_to_id, b);
473 ERR_FAIL_COND_V_MSG(!to_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id));
474
475 if (a == b) {
476 Vector<int64_t> ret;
477 ret.push_back(a->id);
478 return ret;
479 }
480
481 Point *begin_point = a;
482 Point *end_point = b;
483
484 bool found_route = _solve(begin_point, end_point);
485 if (!found_route) {
486 return Vector<int64_t>();
487 }
488
489 Point *p = end_point;
490 int64_t pc = 1; // Begin point
491 while (p != begin_point) {
492 pc++;
493 p = p->prev_point;
494 }
495
496 Vector<int64_t> path;
497 path.resize(pc);
498
499 {
500 int64_t *w = path.ptrw();
501
502 p = end_point;
503 int64_t idx = pc - 1;
504 while (p != begin_point) {
505 w[idx--] = p->id;
506 p = p->prev_point;
507 }
508
509 w[0] = p->id; // Assign first
510 }
511
512 return path;
513}
514
515void AStar3D::set_point_disabled(int64_t p_id, bool p_disabled) {
516 Point *p;
517 bool p_exists = points.lookup(p_id, p);
518 ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set if point is disabled. Point with id: %d doesn't exist.", p_id));
519
520 p->enabled = !p_disabled;
521}
522
523bool AStar3D::is_point_disabled(int64_t p_id) const {
524 Point *p;
525 bool p_exists = points.lookup(p_id, p);
526 ERR_FAIL_COND_V_MSG(!p_exists, false, vformat("Can't get if point is disabled. Point with id: %d doesn't exist.", p_id));
527
528 return !p->enabled;
529}
530
531void AStar3D::_bind_methods() {
532 ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar3D::get_available_point_id);
533 ClassDB::bind_method(D_METHOD("add_point", "id", "position", "weight_scale"), &AStar3D::add_point, DEFVAL(1.0));
534 ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStar3D::get_point_position);
535 ClassDB::bind_method(D_METHOD("set_point_position", "id", "position"), &AStar3D::set_point_position);
536 ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStar3D::get_point_weight_scale);
537 ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar3D::set_point_weight_scale);
538 ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar3D::remove_point);
539 ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar3D::has_point);
540 ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar3D::get_point_connections);
541 ClassDB::bind_method(D_METHOD("get_point_ids"), &AStar3D::get_point_ids);
542
543 ClassDB::bind_method(D_METHOD("set_point_disabled", "id", "disabled"), &AStar3D::set_point_disabled, DEFVAL(true));
544 ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar3D::is_point_disabled);
545
546 ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar3D::connect_points, DEFVAL(true));
547 ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id", "bidirectional"), &AStar3D::disconnect_points, DEFVAL(true));
548 ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id", "bidirectional"), &AStar3D::are_points_connected, DEFVAL(true));
549
550 ClassDB::bind_method(D_METHOD("get_point_count"), &AStar3D::get_point_count);
551 ClassDB::bind_method(D_METHOD("get_point_capacity"), &AStar3D::get_point_capacity);
552 ClassDB::bind_method(D_METHOD("reserve_space", "num_nodes"), &AStar3D::reserve_space);
553 ClassDB::bind_method(D_METHOD("clear"), &AStar3D::clear);
554
555 ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar3D::get_closest_point, DEFVAL(false));
556 ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar3D::get_closest_position_in_segment);
557
558 ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar3D::get_point_path);
559 ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar3D::get_id_path);
560
561 GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id")
562 GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
563}
564
565AStar3D::~AStar3D() {
566 clear();
567}
568
569/////////////////////////////////////////////////////////////
570
571int64_t AStar2D::get_available_point_id() const {
572 return astar.get_available_point_id();
573}
574
575void AStar2D::add_point(int64_t p_id, const Vector2 &p_pos, real_t p_weight_scale) {
576 astar.add_point(p_id, Vector3(p_pos.x, p_pos.y, 0), p_weight_scale);
577}
578
579Vector2 AStar2D::get_point_position(int64_t p_id) const {
580 Vector3 p = astar.get_point_position(p_id);
581 return Vector2(p.x, p.y);
582}
583
584void AStar2D::set_point_position(int64_t p_id, const Vector2 &p_pos) {
585 astar.set_point_position(p_id, Vector3(p_pos.x, p_pos.y, 0));
586}
587
588real_t AStar2D::get_point_weight_scale(int64_t p_id) const {
589 return astar.get_point_weight_scale(p_id);
590}
591
592void AStar2D::set_point_weight_scale(int64_t p_id, real_t p_weight_scale) {
593 astar.set_point_weight_scale(p_id, p_weight_scale);
594}
595
596void AStar2D::remove_point(int64_t p_id) {
597 astar.remove_point(p_id);
598}
599
600bool AStar2D::has_point(int64_t p_id) const {
601 return astar.has_point(p_id);
602}
603
604Vector<int64_t> AStar2D::get_point_connections(int64_t p_id) {
605 return astar.get_point_connections(p_id);
606}
607
608PackedInt64Array AStar2D::get_point_ids() {
609 return astar.get_point_ids();
610}
611
612void AStar2D::set_point_disabled(int64_t p_id, bool p_disabled) {
613 astar.set_point_disabled(p_id, p_disabled);
614}
615
616bool AStar2D::is_point_disabled(int64_t p_id) const {
617 return astar.is_point_disabled(p_id);
618}
619
620void AStar2D::connect_points(int64_t p_id, int64_t p_with_id, bool p_bidirectional) {
621 astar.connect_points(p_id, p_with_id, p_bidirectional);
622}
623
624void AStar2D::disconnect_points(int64_t p_id, int64_t p_with_id, bool p_bidirectional) {
625 astar.disconnect_points(p_id, p_with_id, p_bidirectional);
626}
627
628bool AStar2D::are_points_connected(int64_t p_id, int64_t p_with_id, bool p_bidirectional) const {
629 return astar.are_points_connected(p_id, p_with_id, p_bidirectional);
630}
631
632int64_t AStar2D::get_point_count() const {
633 return astar.get_point_count();
634}
635
636int64_t AStar2D::get_point_capacity() const {
637 return astar.get_point_capacity();
638}
639
640void AStar2D::clear() {
641 astar.clear();
642}
643
644void AStar2D::reserve_space(int64_t p_num_nodes) {
645 astar.reserve_space(p_num_nodes);
646}
647
648int64_t AStar2D::get_closest_point(const Vector2 &p_point, bool p_include_disabled) const {
649 return astar.get_closest_point(Vector3(p_point.x, p_point.y, 0), p_include_disabled);
650}
651
652Vector2 AStar2D::get_closest_position_in_segment(const Vector2 &p_point) const {
653 Vector3 p = astar.get_closest_position_in_segment(Vector3(p_point.x, p_point.y, 0));
654 return Vector2(p.x, p.y);
655}
656
657real_t AStar2D::_estimate_cost(int64_t p_from_id, int64_t p_to_id) {
658 real_t scost;
659 if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_to_id, scost)) {
660 return scost;
661 }
662
663 AStar3D::Point *from_point;
664 bool from_exists = astar.points.lookup(p_from_id, from_point);
665 ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
666
667 AStar3D::Point *to_point;
668 bool to_exists = astar.points.lookup(p_to_id, to_point);
669 ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_to_id));
670
671 return from_point->pos.distance_to(to_point->pos);
672}
673
674real_t AStar2D::_compute_cost(int64_t p_from_id, int64_t p_to_id) {
675 real_t scost;
676 if (GDVIRTUAL_CALL(_compute_cost, p_from_id, p_to_id, scost)) {
677 return scost;
678 }
679
680 AStar3D::Point *from_point;
681 bool from_exists = astar.points.lookup(p_from_id, from_point);
682 ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_from_id));
683
684 AStar3D::Point *to_point;
685 bool to_exists = astar.points.lookup(p_to_id, to_point);
686 ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_to_id));
687
688 return from_point->pos.distance_to(to_point->pos);
689}
690
691Vector<Vector2> AStar2D::get_point_path(int64_t p_from_id, int64_t p_to_id) {
692 AStar3D::Point *a;
693 bool from_exists = astar.points.lookup(p_from_id, a);
694 ERR_FAIL_COND_V_MSG(!from_exists, Vector<Vector2>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_from_id));
695
696 AStar3D::Point *b;
697 bool to_exists = astar.points.lookup(p_to_id, b);
698 ERR_FAIL_COND_V_MSG(!to_exists, Vector<Vector2>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_to_id));
699
700 if (a == b) {
701 Vector<Vector2> ret = { Vector2(a->pos.x, a->pos.y) };
702 return ret;
703 }
704
705 AStar3D::Point *begin_point = a;
706 AStar3D::Point *end_point = b;
707
708 bool found_route = _solve(begin_point, end_point);
709 if (!found_route) {
710 return Vector<Vector2>();
711 }
712
713 AStar3D::Point *p = end_point;
714 int64_t pc = 1; // Begin point
715 while (p != begin_point) {
716 pc++;
717 p = p->prev_point;
718 }
719
720 Vector<Vector2> path;
721 path.resize(pc);
722
723 {
724 Vector2 *w = path.ptrw();
725
726 AStar3D::Point *p2 = end_point;
727 int64_t idx = pc - 1;
728 while (p2 != begin_point) {
729 w[idx--] = Vector2(p2->pos.x, p2->pos.y);
730 p2 = p2->prev_point;
731 }
732
733 w[0] = Vector2(p2->pos.x, p2->pos.y); // Assign first
734 }
735
736 return path;
737}
738
739Vector<int64_t> AStar2D::get_id_path(int64_t p_from_id, int64_t p_to_id) {
740 AStar3D::Point *a;
741 bool from_exists = astar.points.lookup(p_from_id, a);
742 ERR_FAIL_COND_V_MSG(!from_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id));
743
744 AStar3D::Point *b;
745 bool to_exists = astar.points.lookup(p_to_id, b);
746 ERR_FAIL_COND_V_MSG(!to_exists, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id));
747
748 if (a == b) {
749 Vector<int64_t> ret;
750 ret.push_back(a->id);
751 return ret;
752 }
753
754 AStar3D::Point *begin_point = a;
755 AStar3D::Point *end_point = b;
756
757 bool found_route = _solve(begin_point, end_point);
758 if (!found_route) {
759 return Vector<int64_t>();
760 }
761
762 AStar3D::Point *p = end_point;
763 int64_t pc = 1; // Begin point
764 while (p != begin_point) {
765 pc++;
766 p = p->prev_point;
767 }
768
769 Vector<int64_t> path;
770 path.resize(pc);
771
772 {
773 int64_t *w = path.ptrw();
774
775 p = end_point;
776 int64_t idx = pc - 1;
777 while (p != begin_point) {
778 w[idx--] = p->id;
779 p = p->prev_point;
780 }
781
782 w[0] = p->id; // Assign first
783 }
784
785 return path;
786}
787
788bool AStar2D::_solve(AStar3D::Point *begin_point, AStar3D::Point *end_point) {
789 astar.pass++;
790
791 if (!end_point->enabled) {
792 return false;
793 }
794
795 bool found_route = false;
796
797 LocalVector<AStar3D::Point *> open_list;
798 SortArray<AStar3D::Point *, AStar3D::SortPoints> sorter;
799
800 begin_point->g_score = 0;
801 begin_point->f_score = _estimate_cost(begin_point->id, end_point->id);
802 open_list.push_back(begin_point);
803
804 while (!open_list.is_empty()) {
805 AStar3D::Point *p = open_list[0]; // The currently processed point.
806
807 if (p == end_point) {
808 found_route = true;
809 break;
810 }
811
812 sorter.pop_heap(0, open_list.size(), open_list.ptr()); // Remove the current point from the open list.
813 open_list.remove_at(open_list.size() - 1);
814 p->closed_pass = astar.pass; // Mark the point as closed.
815
816 for (OAHashMap<int64_t, AStar3D::Point *>::Iterator it = p->neighbors.iter(); it.valid; it = p->neighbors.next_iter(it)) {
817 AStar3D::Point *e = *(it.value); // The neighbor point.
818
819 if (!e->enabled || e->closed_pass == astar.pass) {
820 continue;
821 }
822
823 real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * e->weight_scale;
824
825 bool new_point = false;
826
827 if (e->open_pass != astar.pass) { // The point wasn't inside the open list.
828 e->open_pass = astar.pass;
829 open_list.push_back(e);
830 new_point = true;
831 } else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous.
832 continue;
833 }
834
835 e->prev_point = p;
836 e->g_score = tentative_g_score;
837 e->f_score = e->g_score + _estimate_cost(e->id, end_point->id);
838
839 if (new_point) { // The position of the new points is already known.
840 sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptr());
841 } else {
842 sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptr());
843 }
844 }
845 }
846
847 return found_route;
848}
849
850void AStar2D::_bind_methods() {
851 ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar2D::get_available_point_id);
852 ClassDB::bind_method(D_METHOD("add_point", "id", "position", "weight_scale"), &AStar2D::add_point, DEFVAL(1.0));
853 ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStar2D::get_point_position);
854 ClassDB::bind_method(D_METHOD("set_point_position", "id", "position"), &AStar2D::set_point_position);
855 ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStar2D::get_point_weight_scale);
856 ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar2D::set_point_weight_scale);
857 ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar2D::remove_point);
858 ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar2D::has_point);
859 ClassDB::bind_method(D_METHOD("get_point_connections", "id"), &AStar2D::get_point_connections);
860 ClassDB::bind_method(D_METHOD("get_point_ids"), &AStar2D::get_point_ids);
861
862 ClassDB::bind_method(D_METHOD("set_point_disabled", "id", "disabled"), &AStar2D::set_point_disabled, DEFVAL(true));
863 ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar2D::is_point_disabled);
864
865 ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar2D::connect_points, DEFVAL(true));
866 ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id", "bidirectional"), &AStar2D::disconnect_points, DEFVAL(true));
867 ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id", "bidirectional"), &AStar2D::are_points_connected, DEFVAL(true));
868
869 ClassDB::bind_method(D_METHOD("get_point_count"), &AStar2D::get_point_count);
870 ClassDB::bind_method(D_METHOD("get_point_capacity"), &AStar2D::get_point_capacity);
871 ClassDB::bind_method(D_METHOD("reserve_space", "num_nodes"), &AStar2D::reserve_space);
872 ClassDB::bind_method(D_METHOD("clear"), &AStar2D::clear);
873
874 ClassDB::bind_method(D_METHOD("get_closest_point", "to_position", "include_disabled"), &AStar2D::get_closest_point, DEFVAL(false));
875 ClassDB::bind_method(D_METHOD("get_closest_position_in_segment", "to_position"), &AStar2D::get_closest_position_in_segment);
876
877 ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar2D::get_point_path);
878 ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar2D::get_id_path);
879
880 GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id")
881 GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
882}
883