1/**************************************************************************/
2/* a_star_grid_2d.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_grid_2d.h"
32
33#include "core/variant/typed_array.h"
34
35#define GET_POINT_UNCHECKED(m_id) points[m_id.y - region.position.y][m_id.x - region.position.x]
36
37static real_t heuristic_euclidian(const Vector2i &p_from, const Vector2i &p_to) {
38 real_t dx = (real_t)ABS(p_to.x - p_from.x);
39 real_t dy = (real_t)ABS(p_to.y - p_from.y);
40 return (real_t)Math::sqrt(dx * dx + dy * dy);
41}
42
43static real_t heuristic_manhattan(const Vector2i &p_from, const Vector2i &p_to) {
44 real_t dx = (real_t)ABS(p_to.x - p_from.x);
45 real_t dy = (real_t)ABS(p_to.y - p_from.y);
46 return dx + dy;
47}
48
49static real_t heuristic_octile(const Vector2i &p_from, const Vector2i &p_to) {
50 real_t dx = (real_t)ABS(p_to.x - p_from.x);
51 real_t dy = (real_t)ABS(p_to.y - p_from.y);
52 real_t F = Math_SQRT2 - 1;
53 return (dx < dy) ? F * dx + dy : F * dy + dx;
54}
55
56static real_t heuristic_chebyshev(const Vector2i &p_from, const Vector2i &p_to) {
57 real_t dx = (real_t)ABS(p_to.x - p_from.x);
58 real_t dy = (real_t)ABS(p_to.y - p_from.y);
59 return MAX(dx, dy);
60}
61
62static real_t (*heuristics[AStarGrid2D::HEURISTIC_MAX])(const Vector2i &, const Vector2i &) = { heuristic_euclidian, heuristic_manhattan, heuristic_octile, heuristic_chebyshev };
63
64void AStarGrid2D::set_region(const Rect2i &p_region) {
65 ERR_FAIL_COND(p_region.size.x < 0 || p_region.size.y < 0);
66 if (p_region != region) {
67 region = p_region;
68 dirty = true;
69 }
70}
71
72Rect2i AStarGrid2D::get_region() const {
73 return region;
74}
75
76void AStarGrid2D::set_size(const Size2i &p_size) {
77 WARN_DEPRECATED_MSG(R"(The "size" property is deprecated, use "region" instead.)");
78 ERR_FAIL_COND(p_size.x < 0 || p_size.y < 0);
79 if (p_size != region.size) {
80 region.size = p_size;
81 dirty = true;
82 }
83}
84
85Size2i AStarGrid2D::get_size() const {
86 return region.size;
87}
88
89void AStarGrid2D::set_offset(const Vector2 &p_offset) {
90 if (!offset.is_equal_approx(p_offset)) {
91 offset = p_offset;
92 dirty = true;
93 }
94}
95
96Vector2 AStarGrid2D::get_offset() const {
97 return offset;
98}
99
100void AStarGrid2D::set_cell_size(const Size2 &p_cell_size) {
101 if (!cell_size.is_equal_approx(p_cell_size)) {
102 cell_size = p_cell_size;
103 dirty = true;
104 }
105}
106
107Size2 AStarGrid2D::get_cell_size() const {
108 return cell_size;
109}
110
111void AStarGrid2D::update() {
112 points.clear();
113 const int64_t end_x = region.position.x + region.size.width;
114 const int64_t end_y = region.position.y + region.size.height;
115 for (int64_t y = region.position.y; y < end_y; y++) {
116 LocalVector<Point> line;
117 for (int64_t x = region.position.x; x < end_x; x++) {
118 line.push_back(Point(Vector2i(x, y), offset + Vector2(x, y) * cell_size));
119 }
120 points.push_back(line);
121 }
122 dirty = false;
123}
124
125bool AStarGrid2D::is_in_bounds(int p_x, int p_y) const {
126 return region.has_point(Vector2i(p_x, p_y));
127}
128
129bool AStarGrid2D::is_in_boundsv(const Vector2i &p_id) const {
130 return region.has_point(p_id);
131}
132
133bool AStarGrid2D::is_dirty() const {
134 return dirty;
135}
136
137void AStarGrid2D::set_jumping_enabled(bool p_enabled) {
138 jumping_enabled = p_enabled;
139}
140
141bool AStarGrid2D::is_jumping_enabled() const {
142 return jumping_enabled;
143}
144
145void AStarGrid2D::set_diagonal_mode(DiagonalMode p_diagonal_mode) {
146 ERR_FAIL_INDEX((int)p_diagonal_mode, (int)DIAGONAL_MODE_MAX);
147 diagonal_mode = p_diagonal_mode;
148}
149
150AStarGrid2D::DiagonalMode AStarGrid2D::get_diagonal_mode() const {
151 return diagonal_mode;
152}
153
154void AStarGrid2D::set_default_compute_heuristic(Heuristic p_heuristic) {
155 ERR_FAIL_INDEX((int)p_heuristic, (int)HEURISTIC_MAX);
156 default_compute_heuristic = p_heuristic;
157}
158
159AStarGrid2D::Heuristic AStarGrid2D::get_default_compute_heuristic() const {
160 return default_compute_heuristic;
161}
162
163void AStarGrid2D::set_default_estimate_heuristic(Heuristic p_heuristic) {
164 ERR_FAIL_INDEX((int)p_heuristic, (int)HEURISTIC_MAX);
165 default_estimate_heuristic = p_heuristic;
166}
167
168AStarGrid2D::Heuristic AStarGrid2D::get_default_estimate_heuristic() const {
169 return default_estimate_heuristic;
170}
171
172void AStarGrid2D::set_point_solid(const Vector2i &p_id, bool p_solid) {
173 ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
174 ERR_FAIL_COND_MSG(!is_in_boundsv(p_id), vformat("Can't set if point is disabled. Point %s out of bounds %s.", p_id, region));
175 GET_POINT_UNCHECKED(p_id).solid = p_solid;
176}
177
178bool AStarGrid2D::is_point_solid(const Vector2i &p_id) const {
179 ERR_FAIL_COND_V_MSG(dirty, false, "Grid is not initialized. Call the update method.");
180 ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_id), false, vformat("Can't get if point is disabled. Point %s out of bounds %s.", p_id, region));
181 return GET_POINT_UNCHECKED(p_id).solid;
182}
183
184void AStarGrid2D::set_point_weight_scale(const Vector2i &p_id, real_t p_weight_scale) {
185 ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
186 ERR_FAIL_COND_MSG(!is_in_boundsv(p_id), vformat("Can't set point's weight scale. Point %s out of bounds %s.", p_id, region));
187 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));
188 GET_POINT_UNCHECKED(p_id).weight_scale = p_weight_scale;
189}
190
191real_t AStarGrid2D::get_point_weight_scale(const Vector2i &p_id) const {
192 ERR_FAIL_COND_V_MSG(dirty, 0, "Grid is not initialized. Call the update method.");
193 ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_id), 0, vformat("Can't get point's weight scale. Point %s out of bounds %s.", p_id, region));
194 return GET_POINT_UNCHECKED(p_id).weight_scale;
195}
196
197void AStarGrid2D::fill_solid_region(const Rect2i &p_region, bool p_solid) {
198 ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
199
200 Rect2i safe_region = p_region.intersection(region);
201 int from_x = safe_region.get_position().x;
202 int from_y = safe_region.get_position().y;
203 int end_x = safe_region.get_end().x;
204 int end_y = safe_region.get_end().y;
205
206 for (int x = from_x; x < end_x; x++) {
207 for (int y = from_y; y < end_y; y++) {
208 GET_POINT_UNCHECKED(Vector2i(x, y)).solid = p_solid;
209 }
210 }
211}
212
213void AStarGrid2D::fill_weight_scale_region(const Rect2i &p_region, real_t p_weight_scale) {
214 ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
215 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));
216
217 Rect2i safe_region = p_region.intersection(region);
218 int from_x = safe_region.get_position().x;
219 int from_y = safe_region.get_position().y;
220 int end_x = safe_region.get_end().x;
221 int end_y = safe_region.get_end().y;
222 for (int x = from_x; x < end_x; x++) {
223 for (int y = from_y; y < end_y; y++) {
224 GET_POINT_UNCHECKED(Vector2i(x, y)).weight_scale = p_weight_scale;
225 }
226 }
227}
228
229AStarGrid2D::Point *AStarGrid2D::_jump(Point *p_from, Point *p_to) {
230 if (!p_to || p_to->solid) {
231 return nullptr;
232 }
233 if (p_to == end) {
234 return p_to;
235 }
236
237 int64_t from_x = p_from->id.x;
238 int64_t from_y = p_from->id.y;
239
240 int64_t to_x = p_to->id.x;
241 int64_t to_y = p_to->id.y;
242
243 int64_t dx = to_x - from_x;
244 int64_t dy = to_y - from_y;
245
246 if (diagonal_mode == DIAGONAL_MODE_ALWAYS || diagonal_mode == DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE) {
247 if (dx != 0 && dy != 0) {
248 if ((_is_walkable(to_x - dx, to_y + dy) && !_is_walkable(to_x - dx, to_y)) || (_is_walkable(to_x + dx, to_y - dy) && !_is_walkable(to_x, to_y - dy))) {
249 return p_to;
250 }
251 if (_jump(p_to, _get_point(to_x + dx, to_y)) != nullptr) {
252 return p_to;
253 }
254 if (_jump(p_to, _get_point(to_x, to_y + dy)) != nullptr) {
255 return p_to;
256 }
257 } else {
258 if (dx != 0) {
259 if ((_is_walkable(to_x + dx, to_y + 1) && !_is_walkable(to_x, to_y + 1)) || (_is_walkable(to_x + dx, to_y - 1) && !_is_walkable(to_x, to_y - 1))) {
260 return p_to;
261 }
262 } else {
263 if ((_is_walkable(to_x + 1, to_y + dy) && !_is_walkable(to_x + 1, to_y)) || (_is_walkable(to_x - 1, to_y + dy) && !_is_walkable(to_x - 1, to_y))) {
264 return p_to;
265 }
266 }
267 }
268 if (_is_walkable(to_x + dx, to_y + dy) && (diagonal_mode == DIAGONAL_MODE_ALWAYS || (_is_walkable(to_x + dx, to_y) || _is_walkable(to_x, to_y + dy)))) {
269 return _jump(p_to, _get_point(to_x + dx, to_y + dy));
270 }
271 } else if (diagonal_mode == DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES) {
272 if (dx != 0 && dy != 0) {
273 if ((_is_walkable(to_x + dx, to_y + dy) && !_is_walkable(to_x, to_y + dy)) || !_is_walkable(to_x + dx, to_y)) {
274 return p_to;
275 }
276 if (_jump(p_to, _get_point(to_x + dx, to_y)) != nullptr) {
277 return p_to;
278 }
279 if (_jump(p_to, _get_point(to_x, to_y + dy)) != nullptr) {
280 return p_to;
281 }
282 } else {
283 if (dx != 0) {
284 if ((_is_walkable(to_x, to_y + 1) && !_is_walkable(to_x - dx, to_y + 1)) || (_is_walkable(to_x, to_y - 1) && !_is_walkable(to_x - dx, to_y - 1))) {
285 return p_to;
286 }
287 } else {
288 if ((_is_walkable(to_x + 1, to_y) && !_is_walkable(to_x + 1, to_y - dy)) || (_is_walkable(to_x - 1, to_y) && !_is_walkable(to_x - 1, to_y - dy))) {
289 return p_to;
290 }
291 }
292 }
293 if (_is_walkable(to_x + dx, to_y + dy) && _is_walkable(to_x + dx, to_y) && _is_walkable(to_x, to_y + dy)) {
294 return _jump(p_to, _get_point(to_x + dx, to_y + dy));
295 }
296 } else { // DIAGONAL_MODE_NEVER
297 if (dx != 0) {
298 if ((_is_walkable(to_x, to_y - 1) && !_is_walkable(to_x - dx, to_y - 1)) || (_is_walkable(to_x, to_y + 1) && !_is_walkable(to_x - dx, to_y + 1))) {
299 return p_to;
300 }
301 } else if (dy != 0) {
302 if ((_is_walkable(to_x - 1, to_y) && !_is_walkable(to_x - 1, to_y - dy)) || (_is_walkable(to_x + 1, to_y) && !_is_walkable(to_x + 1, to_y - dy))) {
303 return p_to;
304 }
305 if (_jump(p_to, _get_point(to_x + 1, to_y)) != nullptr) {
306 return p_to;
307 }
308 if (_jump(p_to, _get_point(to_x - 1, to_y)) != nullptr) {
309 return p_to;
310 }
311 }
312 return _jump(p_to, _get_point(to_x + dx, to_y + dy));
313 }
314 return nullptr;
315}
316
317void AStarGrid2D::_get_nbors(Point *p_point, LocalVector<Point *> &r_nbors) {
318 bool ts0 = false, td0 = false,
319 ts1 = false, td1 = false,
320 ts2 = false, td2 = false,
321 ts3 = false, td3 = false;
322
323 Point *left = nullptr;
324 Point *right = nullptr;
325 Point *top = nullptr;
326 Point *bottom = nullptr;
327
328 Point *top_left = nullptr;
329 Point *top_right = nullptr;
330 Point *bottom_left = nullptr;
331 Point *bottom_right = nullptr;
332
333 {
334 bool has_left = false;
335 bool has_right = false;
336
337 if (p_point->id.x - 1 >= region.position.x) {
338 left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y);
339 has_left = true;
340 }
341 if (p_point->id.x + 1 < region.position.x + region.size.width) {
342 right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y);
343 has_right = true;
344 }
345 if (p_point->id.y - 1 >= region.position.y) {
346 top = _get_point_unchecked(p_point->id.x, p_point->id.y - 1);
347 if (has_left) {
348 top_left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y - 1);
349 }
350 if (has_right) {
351 top_right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y - 1);
352 }
353 }
354 if (p_point->id.y + 1 < region.position.y + region.size.height) {
355 bottom = _get_point_unchecked(p_point->id.x, p_point->id.y + 1);
356 if (has_left) {
357 bottom_left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y + 1);
358 }
359 if (has_right) {
360 bottom_right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y + 1);
361 }
362 }
363 }
364
365 if (top && !top->solid) {
366 r_nbors.push_back(top);
367 ts0 = true;
368 }
369 if (right && !right->solid) {
370 r_nbors.push_back(right);
371 ts1 = true;
372 }
373 if (bottom && !bottom->solid) {
374 r_nbors.push_back(bottom);
375 ts2 = true;
376 }
377 if (left && !left->solid) {
378 r_nbors.push_back(left);
379 ts3 = true;
380 }
381
382 switch (diagonal_mode) {
383 case DIAGONAL_MODE_ALWAYS: {
384 td0 = true;
385 td1 = true;
386 td2 = true;
387 td3 = true;
388 } break;
389 case DIAGONAL_MODE_NEVER: {
390 } break;
391 case DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE: {
392 td0 = ts3 || ts0;
393 td1 = ts0 || ts1;
394 td2 = ts1 || ts2;
395 td3 = ts2 || ts3;
396 } break;
397 case DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES: {
398 td0 = ts3 && ts0;
399 td1 = ts0 && ts1;
400 td2 = ts1 && ts2;
401 td3 = ts2 && ts3;
402 } break;
403 default:
404 break;
405 }
406
407 if (td0 && (top_left && !top_left->solid)) {
408 r_nbors.push_back(top_left);
409 }
410 if (td1 && (top_right && !top_right->solid)) {
411 r_nbors.push_back(top_right);
412 }
413 if (td2 && (bottom_right && !bottom_right->solid)) {
414 r_nbors.push_back(bottom_right);
415 }
416 if (td3 && (bottom_left && !bottom_left->solid)) {
417 r_nbors.push_back(bottom_left);
418 }
419}
420
421bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
422 pass++;
423
424 if (p_end_point->solid) {
425 return false;
426 }
427
428 bool found_route = false;
429
430 LocalVector<Point *> open_list;
431 SortArray<Point *, SortPoints> sorter;
432
433 p_begin_point->g_score = 0;
434 p_begin_point->f_score = _estimate_cost(p_begin_point->id, p_end_point->id);
435 open_list.push_back(p_begin_point);
436 end = p_end_point;
437
438 while (!open_list.is_empty()) {
439 Point *p = open_list[0]; // The currently processed point.
440
441 if (p == p_end_point) {
442 found_route = true;
443 break;
444 }
445
446 sorter.pop_heap(0, open_list.size(), open_list.ptr()); // Remove the current point from the open list.
447 open_list.remove_at(open_list.size() - 1);
448 p->closed_pass = pass; // Mark the point as closed.
449
450 LocalVector<Point *> nbors;
451 _get_nbors(p, nbors);
452
453 for (Point *e : nbors) {
454 real_t weight_scale = 1.0;
455
456 if (jumping_enabled) {
457 // TODO: Make it works with weight_scale.
458 e = _jump(p, e);
459 if (!e || e->closed_pass == pass) {
460 continue;
461 }
462 } else {
463 if (e->solid || e->closed_pass == pass) {
464 continue;
465 }
466 weight_scale = e->weight_scale;
467 }
468
469 real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * weight_scale;
470 bool new_point = false;
471
472 if (e->open_pass != pass) { // The point wasn't inside the open list.
473 e->open_pass = pass;
474 open_list.push_back(e);
475 new_point = true;
476 } else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous.
477 continue;
478 }
479
480 e->prev_point = p;
481 e->g_score = tentative_g_score;
482 e->f_score = e->g_score + _estimate_cost(e->id, p_end_point->id);
483
484 if (new_point) { // The position of the new points is already known.
485 sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptr());
486 } else {
487 sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptr());
488 }
489 }
490 }
491
492 return found_route;
493}
494
495real_t AStarGrid2D::_estimate_cost(const Vector2i &p_from_id, const Vector2i &p_to_id) {
496 real_t scost;
497 if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_to_id, scost)) {
498 return scost;
499 }
500 return heuristics[default_estimate_heuristic](p_from_id, p_to_id);
501}
502
503real_t AStarGrid2D::_compute_cost(const Vector2i &p_from_id, const Vector2i &p_to_id) {
504 real_t scost;
505 if (GDVIRTUAL_CALL(_compute_cost, p_from_id, p_to_id, scost)) {
506 return scost;
507 }
508 return heuristics[default_compute_heuristic](p_from_id, p_to_id);
509}
510
511void AStarGrid2D::clear() {
512 points.clear();
513 region = Rect2i();
514}
515
516Vector2 AStarGrid2D::get_point_position(const Vector2i &p_id) const {
517 ERR_FAIL_COND_V_MSG(dirty, Vector2(), "Grid is not initialized. Call the update method.");
518 ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_id), Vector2(), vformat("Can't get point's position. Point %s out of bounds %s.", p_id, region));
519 return GET_POINT_UNCHECKED(p_id).pos;
520}
521
522Vector<Vector2> AStarGrid2D::get_point_path(const Vector2i &p_from_id, const Vector2i &p_to_id) {
523 ERR_FAIL_COND_V_MSG(dirty, Vector<Vector2>(), "Grid is not initialized. Call the update method.");
524 ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_from_id), Vector<Vector2>(), vformat("Can't get id path. Point %s out of bounds %s.", p_from_id, region));
525 ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_to_id), Vector<Vector2>(), vformat("Can't get id path. Point %s out of bounds %s.", p_to_id, region));
526
527 Point *a = _get_point(p_from_id.x, p_from_id.y);
528 Point *b = _get_point(p_to_id.x, p_to_id.y);
529
530 if (a == b) {
531 Vector<Vector2> ret;
532 ret.push_back(a->pos);
533 return ret;
534 }
535
536 Point *begin_point = a;
537 Point *end_point = b;
538
539 bool found_route = _solve(begin_point, end_point);
540 if (!found_route) {
541 return Vector<Vector2>();
542 }
543
544 Point *p = end_point;
545 int64_t pc = 1;
546 while (p != begin_point) {
547 pc++;
548 p = p->prev_point;
549 }
550
551 Vector<Vector2> path;
552 path.resize(pc);
553
554 {
555 Vector2 *w = path.ptrw();
556
557 p = end_point;
558 int64_t idx = pc - 1;
559 while (p != begin_point) {
560 w[idx--] = p->pos;
561 p = p->prev_point;
562 }
563
564 w[0] = p->pos;
565 }
566
567 return path;
568}
569
570TypedArray<Vector2i> AStarGrid2D::get_id_path(const Vector2i &p_from_id, const Vector2i &p_to_id) {
571 ERR_FAIL_COND_V_MSG(dirty, TypedArray<Vector2i>(), "Grid is not initialized. Call the update method.");
572 ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_from_id), TypedArray<Vector2i>(), vformat("Can't get id path. Point %s out of bounds %s.", p_from_id, region));
573 ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_to_id), TypedArray<Vector2i>(), vformat("Can't get id path. Point %s out of bounds %s.", p_to_id, region));
574
575 Point *a = _get_point(p_from_id.x, p_from_id.y);
576 Point *b = _get_point(p_to_id.x, p_to_id.y);
577
578 if (a == b) {
579 TypedArray<Vector2i> ret;
580 ret.push_back(a->id);
581 return ret;
582 }
583
584 Point *begin_point = a;
585 Point *end_point = b;
586
587 bool found_route = _solve(begin_point, end_point);
588 if (!found_route) {
589 return TypedArray<Vector2i>();
590 }
591
592 Point *p = end_point;
593 int64_t pc = 1;
594 while (p != begin_point) {
595 pc++;
596 p = p->prev_point;
597 }
598
599 TypedArray<Vector2i> path;
600 path.resize(pc);
601
602 {
603 p = end_point;
604 int64_t idx = pc - 1;
605 while (p != begin_point) {
606 path[idx--] = p->id;
607 p = p->prev_point;
608 }
609
610 path[0] = p->id;
611 }
612
613 return path;
614}
615
616void AStarGrid2D::_bind_methods() {
617 ClassDB::bind_method(D_METHOD("set_region", "region"), &AStarGrid2D::set_region);
618 ClassDB::bind_method(D_METHOD("get_region"), &AStarGrid2D::get_region);
619 ClassDB::bind_method(D_METHOD("set_size", "size"), &AStarGrid2D::set_size);
620 ClassDB::bind_method(D_METHOD("get_size"), &AStarGrid2D::get_size);
621 ClassDB::bind_method(D_METHOD("set_offset", "offset"), &AStarGrid2D::set_offset);
622 ClassDB::bind_method(D_METHOD("get_offset"), &AStarGrid2D::get_offset);
623 ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &AStarGrid2D::set_cell_size);
624 ClassDB::bind_method(D_METHOD("get_cell_size"), &AStarGrid2D::get_cell_size);
625 ClassDB::bind_method(D_METHOD("is_in_bounds", "x", "y"), &AStarGrid2D::is_in_bounds);
626 ClassDB::bind_method(D_METHOD("is_in_boundsv", "id"), &AStarGrid2D::is_in_boundsv);
627 ClassDB::bind_method(D_METHOD("is_dirty"), &AStarGrid2D::is_dirty);
628 ClassDB::bind_method(D_METHOD("update"), &AStarGrid2D::update);
629 ClassDB::bind_method(D_METHOD("set_jumping_enabled", "enabled"), &AStarGrid2D::set_jumping_enabled);
630 ClassDB::bind_method(D_METHOD("is_jumping_enabled"), &AStarGrid2D::is_jumping_enabled);
631 ClassDB::bind_method(D_METHOD("set_diagonal_mode", "mode"), &AStarGrid2D::set_diagonal_mode);
632 ClassDB::bind_method(D_METHOD("get_diagonal_mode"), &AStarGrid2D::get_diagonal_mode);
633 ClassDB::bind_method(D_METHOD("set_default_compute_heuristic", "heuristic"), &AStarGrid2D::set_default_compute_heuristic);
634 ClassDB::bind_method(D_METHOD("get_default_compute_heuristic"), &AStarGrid2D::get_default_compute_heuristic);
635 ClassDB::bind_method(D_METHOD("set_default_estimate_heuristic", "heuristic"), &AStarGrid2D::set_default_estimate_heuristic);
636 ClassDB::bind_method(D_METHOD("get_default_estimate_heuristic"), &AStarGrid2D::get_default_estimate_heuristic);
637 ClassDB::bind_method(D_METHOD("set_point_solid", "id", "solid"), &AStarGrid2D::set_point_solid, DEFVAL(true));
638 ClassDB::bind_method(D_METHOD("is_point_solid", "id"), &AStarGrid2D::is_point_solid);
639 ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStarGrid2D::set_point_weight_scale);
640 ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStarGrid2D::get_point_weight_scale);
641 ClassDB::bind_method(D_METHOD("fill_solid_region", "region", "solid"), &AStarGrid2D::fill_solid_region, DEFVAL(true));
642 ClassDB::bind_method(D_METHOD("fill_weight_scale_region", "region", "weight_scale"), &AStarGrid2D::fill_weight_scale_region);
643 ClassDB::bind_method(D_METHOD("clear"), &AStarGrid2D::clear);
644
645 ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStarGrid2D::get_point_position);
646 ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStarGrid2D::get_point_path);
647 ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStarGrid2D::get_id_path);
648
649 GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id")
650 GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
651
652 ADD_PROPERTY(PropertyInfo(Variant::RECT2I, "region"), "set_region", "get_region");
653 ADD_PROPERTY(PropertyInfo(Variant::VECTOR2I, "size"), "set_size", "get_size");
654 ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "offset"), "set_offset", "get_offset");
655 ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "cell_size"), "set_cell_size", "get_cell_size");
656
657 ADD_PROPERTY(PropertyInfo(Variant::BOOL, "jumping_enabled"), "set_jumping_enabled", "is_jumping_enabled");
658 ADD_PROPERTY(PropertyInfo(Variant::INT, "default_compute_heuristic", PROPERTY_HINT_ENUM, "Euclidean,Manhattan,Octile,Chebyshev"), "set_default_compute_heuristic", "get_default_compute_heuristic");
659 ADD_PROPERTY(PropertyInfo(Variant::INT, "default_estimate_heuristic", PROPERTY_HINT_ENUM, "Euclidean,Manhattan,Octile,Chebyshev"), "set_default_estimate_heuristic", "get_default_estimate_heuristic");
660 ADD_PROPERTY(PropertyInfo(Variant::INT, "diagonal_mode", PROPERTY_HINT_ENUM, "Never,Always,At Least One Walkable,Only If No Obstacles"), "set_diagonal_mode", "get_diagonal_mode");
661
662 BIND_ENUM_CONSTANT(HEURISTIC_EUCLIDEAN);
663 BIND_ENUM_CONSTANT(HEURISTIC_MANHATTAN);
664 BIND_ENUM_CONSTANT(HEURISTIC_OCTILE);
665 BIND_ENUM_CONSTANT(HEURISTIC_CHEBYSHEV);
666 BIND_ENUM_CONSTANT(HEURISTIC_MAX);
667
668 BIND_ENUM_CONSTANT(DIAGONAL_MODE_ALWAYS);
669 BIND_ENUM_CONSTANT(DIAGONAL_MODE_NEVER);
670 BIND_ENUM_CONSTANT(DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE);
671 BIND_ENUM_CONSTANT(DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES);
672 BIND_ENUM_CONSTANT(DIAGONAL_MODE_MAX);
673}
674
675#undef GET_POINT_UNCHECKED
676