a_star_grid_2d.cpp 20 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608
  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) 2007-2022 Juan Linietsky, Ariel Manzur. */
  9. /* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */
  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. #include "a_star_grid_2d.h"
  31. #include "core/variant/typed_array.h"
  32. static real_t heuristic_euclidian(const Vector2i &p_from, const Vector2i &p_to) {
  33. real_t dx = (real_t)ABS(p_to.x - p_from.x);
  34. real_t dy = (real_t)ABS(p_to.y - p_from.y);
  35. return (real_t)Math::sqrt(dx * dx + dy * dy);
  36. }
  37. static real_t heuristic_manhattan(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 dx + dy;
  41. }
  42. static real_t heuristic_octile(const Vector2i &p_from, const Vector2i &p_to) {
  43. real_t dx = (real_t)ABS(p_to.x - p_from.x);
  44. real_t dy = (real_t)ABS(p_to.y - p_from.y);
  45. real_t F = Math_SQRT2 - 1;
  46. return (dx < dy) ? F * dx + dy : F * dy + dx;
  47. }
  48. static real_t heuristic_chebyshev(const Vector2i &p_from, const Vector2i &p_to) {
  49. real_t dx = (real_t)ABS(p_to.x - p_from.x);
  50. real_t dy = (real_t)ABS(p_to.y - p_from.y);
  51. return MAX(dx, dy);
  52. }
  53. static real_t (*heuristics[AStarGrid2D::HEURISTIC_MAX])(const Vector2i &, const Vector2i &) = { heuristic_euclidian, heuristic_manhattan, heuristic_octile, heuristic_chebyshev };
  54. void AStarGrid2D::set_size(const Size2i &p_size) {
  55. ERR_FAIL_COND(p_size.x < 0 || p_size.y < 0);
  56. if (p_size != size) {
  57. size = p_size;
  58. dirty = true;
  59. }
  60. }
  61. Size2i AStarGrid2D::get_size() const {
  62. return size;
  63. }
  64. void AStarGrid2D::set_offset(const Vector2 &p_offset) {
  65. if (!offset.is_equal_approx(p_offset)) {
  66. offset = p_offset;
  67. dirty = true;
  68. }
  69. }
  70. Vector2 AStarGrid2D::get_offset() const {
  71. return offset;
  72. }
  73. void AStarGrid2D::set_cell_size(const Size2 &p_cell_size) {
  74. if (!cell_size.is_equal_approx(p_cell_size)) {
  75. cell_size = p_cell_size;
  76. dirty = true;
  77. }
  78. }
  79. Size2 AStarGrid2D::get_cell_size() const {
  80. return cell_size;
  81. }
  82. void AStarGrid2D::update() {
  83. points.clear();
  84. for (int64_t y = 0; y < size.y; y++) {
  85. LocalVector<Point> line;
  86. for (int64_t x = 0; x < size.x; x++) {
  87. line.push_back(Point(Vector2i(x, y), offset + Vector2(x, y) * cell_size));
  88. }
  89. points.push_back(line);
  90. }
  91. dirty = false;
  92. }
  93. bool AStarGrid2D::is_in_bounds(int p_x, int p_y) const {
  94. return p_x >= 0 && p_x < size.width && p_y >= 0 && p_y < size.height;
  95. }
  96. bool AStarGrid2D::is_in_boundsv(const Vector2i &p_id) const {
  97. return p_id.x >= 0 && p_id.x < size.width && p_id.y >= 0 && p_id.y < size.height;
  98. }
  99. bool AStarGrid2D::is_dirty() const {
  100. return dirty;
  101. }
  102. void AStarGrid2D::set_jumping_enabled(bool p_enabled) {
  103. jumping_enabled = p_enabled;
  104. }
  105. bool AStarGrid2D::is_jumping_enabled() const {
  106. return jumping_enabled;
  107. }
  108. void AStarGrid2D::set_diagonal_mode(DiagonalMode p_diagonal_mode) {
  109. ERR_FAIL_INDEX((int)p_diagonal_mode, (int)DIAGONAL_MODE_MAX);
  110. diagonal_mode = p_diagonal_mode;
  111. }
  112. AStarGrid2D::DiagonalMode AStarGrid2D::get_diagonal_mode() const {
  113. return diagonal_mode;
  114. }
  115. void AStarGrid2D::set_default_heuristic(Heuristic p_heuristic) {
  116. ERR_FAIL_INDEX((int)p_heuristic, (int)HEURISTIC_MAX);
  117. default_heuristic = p_heuristic;
  118. }
  119. AStarGrid2D::Heuristic AStarGrid2D::get_default_heuristic() const {
  120. return default_heuristic;
  121. }
  122. void AStarGrid2D::set_point_solid(const Vector2i &p_id, bool p_solid) {
  123. ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
  124. ERR_FAIL_COND_MSG(!is_in_boundsv(p_id), vformat("Can't set if point is disabled. Point out of bounds (%s/%s, %s/%s).", p_id.x, size.width, p_id.y, size.height));
  125. points[p_id.y][p_id.x].solid = p_solid;
  126. }
  127. bool AStarGrid2D::is_point_solid(const Vector2i &p_id) const {
  128. ERR_FAIL_COND_V_MSG(dirty, false, "Grid is not initialized. Call the update method.");
  129. ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_id), false, vformat("Can't get if point is disabled. Point out of bounds (%s/%s, %s/%s).", p_id.x, size.width, p_id.y, size.height));
  130. return points[p_id.y][p_id.x].solid;
  131. }
  132. void AStarGrid2D::set_point_weight_scale(const Vector2i &p_id, real_t p_weight_scale) {
  133. ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
  134. ERR_FAIL_COND_MSG(!is_in_boundsv(p_id), vformat("Can't set point's weight scale. Point out of bounds (%s/%s, %s/%s).", p_id.x, size.width, p_id.y, size.height));
  135. 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));
  136. points[p_id.y][p_id.x].weight_scale = p_weight_scale;
  137. }
  138. real_t AStarGrid2D::get_point_weight_scale(const Vector2i &p_id) const {
  139. ERR_FAIL_COND_V_MSG(dirty, 0, "Grid is not initialized. Call the update method.");
  140. ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_id), 0, vformat("Can't get point's weight scale. Point out of bounds (%s/%s, %s/%s).", p_id.x, size.width, p_id.y, size.height));
  141. return points[p_id.y][p_id.x].weight_scale;
  142. }
  143. AStarGrid2D::Point *AStarGrid2D::_jump(Point *p_from, Point *p_to) {
  144. if (!p_to || p_to->solid) {
  145. return nullptr;
  146. }
  147. if (p_to == end) {
  148. return p_to;
  149. }
  150. int64_t from_x = p_from->id.x;
  151. int64_t from_y = p_from->id.y;
  152. int64_t to_x = p_to->id.x;
  153. int64_t to_y = p_to->id.y;
  154. int64_t dx = to_x - from_x;
  155. int64_t dy = to_y - from_y;
  156. if (diagonal_mode == DIAGONAL_MODE_ALWAYS || diagonal_mode == DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE) {
  157. if (dx != 0 && dy != 0) {
  158. 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))) {
  159. return p_to;
  160. }
  161. if (_jump(p_to, _get_point(to_x + dx, to_y)) != nullptr) {
  162. return p_to;
  163. }
  164. if (_jump(p_to, _get_point(to_x, to_y + dy)) != nullptr) {
  165. return p_to;
  166. }
  167. } else {
  168. if (dx != 0) {
  169. 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))) {
  170. return p_to;
  171. }
  172. } else {
  173. 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))) {
  174. return p_to;
  175. }
  176. }
  177. }
  178. 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)))) {
  179. return _jump(p_to, _get_point(to_x + dx, to_y + dy));
  180. }
  181. } else if (diagonal_mode == DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES) {
  182. if (dx != 0 && dy != 0) {
  183. if ((_is_walkable(to_x + dx, to_y + dy) && !_is_walkable(to_x, to_y + dy)) || !_is_walkable(to_x + dx, to_y)) {
  184. return p_to;
  185. }
  186. if (_jump(p_to, _get_point(to_x + dx, to_y)) != nullptr) {
  187. return p_to;
  188. }
  189. if (_jump(p_to, _get_point(to_x, to_y + dy)) != nullptr) {
  190. return p_to;
  191. }
  192. } else {
  193. if (dx != 0) {
  194. 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))) {
  195. return p_to;
  196. }
  197. } else {
  198. 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))) {
  199. return p_to;
  200. }
  201. }
  202. }
  203. if (_is_walkable(to_x + dx, to_y + dy) && _is_walkable(to_x + dx, to_y) && _is_walkable(to_x, to_y + dy)) {
  204. return _jump(p_to, _get_point(to_x + dx, to_y + dy));
  205. }
  206. } else { // DIAGONAL_MODE_NEVER
  207. if (dx != 0) {
  208. if (!_is_walkable(to_x + dx, to_y)) {
  209. return p_to;
  210. }
  211. if (_jump(p_to, _get_point(to_x, to_y + 1)) != nullptr) {
  212. return p_to;
  213. }
  214. if (_jump(p_to, _get_point(to_x, to_y - 1)) != nullptr) {
  215. return p_to;
  216. }
  217. } else {
  218. if (!_is_walkable(to_x, to_y + dy)) {
  219. return p_to;
  220. }
  221. if (_jump(p_to, _get_point(to_x + 1, to_y)) != nullptr) {
  222. return p_to;
  223. }
  224. if (_jump(p_to, _get_point(to_x - 1, to_y)) != nullptr) {
  225. return p_to;
  226. }
  227. }
  228. if (_is_walkable(to_x + dx, to_y + dy) && _is_walkable(to_x + dx, to_y) && _is_walkable(to_x, to_y + dy)) {
  229. return _jump(p_to, _get_point(to_x + dx, to_y + dy));
  230. }
  231. }
  232. return nullptr;
  233. }
  234. void AStarGrid2D::_get_nbors(Point *p_point, List<Point *> &r_nbors) {
  235. bool ts0 = false, td0 = false,
  236. ts1 = false, td1 = false,
  237. ts2 = false, td2 = false,
  238. ts3 = false, td3 = false;
  239. Point *left = nullptr;
  240. Point *right = nullptr;
  241. Point *top = nullptr;
  242. Point *bottom = nullptr;
  243. Point *top_left = nullptr;
  244. Point *top_right = nullptr;
  245. Point *bottom_left = nullptr;
  246. Point *bottom_right = nullptr;
  247. {
  248. bool has_left = false;
  249. bool has_right = false;
  250. if (p_point->id.x - 1 >= 0) {
  251. left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y);
  252. has_left = true;
  253. }
  254. if (p_point->id.x + 1 < size.width) {
  255. right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y);
  256. has_right = true;
  257. }
  258. if (p_point->id.y - 1 >= 0) {
  259. top = _get_point_unchecked(p_point->id.x, p_point->id.y - 1);
  260. if (has_left) {
  261. top_left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y - 1);
  262. }
  263. if (has_right) {
  264. top_right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y - 1);
  265. }
  266. }
  267. if (p_point->id.y + 1 < size.height) {
  268. bottom = _get_point_unchecked(p_point->id.x, p_point->id.y + 1);
  269. if (has_left) {
  270. bottom_left = _get_point_unchecked(p_point->id.x - 1, p_point->id.y + 1);
  271. }
  272. if (has_right) {
  273. bottom_right = _get_point_unchecked(p_point->id.x + 1, p_point->id.y + 1);
  274. }
  275. }
  276. }
  277. if (top && !top->solid) {
  278. r_nbors.push_back(top);
  279. ts0 = true;
  280. }
  281. if (right && !right->solid) {
  282. r_nbors.push_back(right);
  283. ts1 = true;
  284. }
  285. if (bottom && !bottom->solid) {
  286. r_nbors.push_back(bottom);
  287. ts2 = true;
  288. }
  289. if (left && !left->solid) {
  290. r_nbors.push_back(left);
  291. ts3 = true;
  292. }
  293. switch (diagonal_mode) {
  294. case DIAGONAL_MODE_ALWAYS: {
  295. td0 = true;
  296. td1 = true;
  297. td2 = true;
  298. td3 = true;
  299. } break;
  300. case DIAGONAL_MODE_NEVER: {
  301. } break;
  302. case DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE: {
  303. td0 = ts3 || ts0;
  304. td1 = ts0 || ts1;
  305. td2 = ts1 || ts2;
  306. td3 = ts2 || ts3;
  307. } break;
  308. case DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES: {
  309. td0 = ts3 && ts0;
  310. td1 = ts0 && ts1;
  311. td2 = ts1 && ts2;
  312. td3 = ts2 && ts3;
  313. } break;
  314. default:
  315. break;
  316. }
  317. if (td0 && (top_left && !top_left->solid)) {
  318. r_nbors.push_back(top_left);
  319. }
  320. if (td1 && (top_right && !top_right->solid)) {
  321. r_nbors.push_back(top_right);
  322. }
  323. if (td2 && (bottom_right && !bottom_right->solid)) {
  324. r_nbors.push_back(bottom_right);
  325. }
  326. if (td3 && (bottom_left && !bottom_left->solid)) {
  327. r_nbors.push_back(bottom_left);
  328. }
  329. }
  330. bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
  331. pass++;
  332. if (p_end_point->solid) {
  333. return false;
  334. }
  335. bool found_route = false;
  336. Vector<Point *> open_list;
  337. SortArray<Point *, SortPoints> sorter;
  338. p_begin_point->g_score = 0;
  339. p_begin_point->f_score = _estimate_cost(p_begin_point->id, p_end_point->id);
  340. open_list.push_back(p_begin_point);
  341. end = p_end_point;
  342. while (!open_list.is_empty()) {
  343. Point *p = open_list[0]; // The currently processed point.
  344. if (p == p_end_point) {
  345. found_route = true;
  346. break;
  347. }
  348. sorter.pop_heap(0, open_list.size(), open_list.ptrw()); // Remove the current point from the open list.
  349. open_list.remove_at(open_list.size() - 1);
  350. p->closed_pass = pass; // Mark the point as closed.
  351. List<Point *> nbors;
  352. _get_nbors(p, nbors);
  353. for (List<Point *>::Element *E = nbors.front(); E; E = E->next()) {
  354. Point *e = E->get(); // The neighbour point.
  355. real_t weight_scale = 1.0;
  356. if (jumping_enabled) {
  357. // TODO: Make it works with weight_scale.
  358. e = _jump(p, e);
  359. if (!e || e->closed_pass == pass) {
  360. continue;
  361. }
  362. } else {
  363. if (e->solid || e->closed_pass == pass) {
  364. continue;
  365. }
  366. weight_scale = e->weight_scale;
  367. }
  368. real_t tentative_g_score = p->g_score + _compute_cost(p->id, e->id) * weight_scale;
  369. bool new_point = false;
  370. if (e->open_pass != pass) { // The point wasn't inside the open list.
  371. e->open_pass = pass;
  372. open_list.push_back(e);
  373. new_point = true;
  374. } else if (tentative_g_score >= e->g_score) { // The new path is worse than the previous.
  375. continue;
  376. }
  377. e->prev_point = p;
  378. e->g_score = tentative_g_score;
  379. e->f_score = e->g_score + _estimate_cost(e->id, p_end_point->id);
  380. if (new_point) { // The position of the new points is already known.
  381. sorter.push_heap(0, open_list.size() - 1, 0, e, open_list.ptrw());
  382. } else {
  383. sorter.push_heap(0, open_list.find(e), 0, e, open_list.ptrw());
  384. }
  385. }
  386. }
  387. return found_route;
  388. }
  389. real_t AStarGrid2D::_estimate_cost(const Vector2i &p_from_id, const Vector2i &p_to_id) {
  390. real_t scost;
  391. if (GDVIRTUAL_CALL(_estimate_cost, p_from_id, p_to_id, scost)) {
  392. return scost;
  393. }
  394. return heuristics[default_heuristic](p_from_id, p_to_id);
  395. }
  396. real_t AStarGrid2D::_compute_cost(const Vector2i &p_from_id, const Vector2i &p_to_id) {
  397. real_t scost;
  398. if (GDVIRTUAL_CALL(_compute_cost, p_from_id, p_to_id, scost)) {
  399. return scost;
  400. }
  401. return heuristics[default_heuristic](p_from_id, p_to_id);
  402. }
  403. void AStarGrid2D::clear() {
  404. points.clear();
  405. size = Vector2i();
  406. }
  407. Vector<Vector2> AStarGrid2D::get_point_path(const Vector2i &p_from_id, const Vector2i &p_to_id) {
  408. ERR_FAIL_COND_V_MSG(dirty, Vector<Vector2>(), "Grid is not initialized. Call the update method.");
  409. ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_from_id), Vector<Vector2>(), vformat("Can't get id path. Point out of bounds (%s/%s, %s/%s)", p_from_id.x, size.width, p_from_id.y, size.height));
  410. ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_to_id), Vector<Vector2>(), vformat("Can't get id path. Point out of bounds (%s/%s, %s/%s)", p_to_id.x, size.width, p_to_id.y, size.height));
  411. Point *a = _get_point(p_from_id.x, p_from_id.y);
  412. Point *b = _get_point(p_to_id.x, p_to_id.y);
  413. if (a == b) {
  414. Vector<Vector2> ret;
  415. ret.push_back(a->pos);
  416. return ret;
  417. }
  418. Point *begin_point = a;
  419. Point *end_point = b;
  420. bool found_route = _solve(begin_point, end_point);
  421. if (!found_route) {
  422. return Vector<Vector2>();
  423. }
  424. Point *p = end_point;
  425. int64_t pc = 1;
  426. while (p != begin_point) {
  427. pc++;
  428. p = p->prev_point;
  429. }
  430. Vector<Vector2> path;
  431. path.resize(pc);
  432. {
  433. Vector2 *w = path.ptrw();
  434. p = end_point;
  435. int64_t idx = pc - 1;
  436. while (p != begin_point) {
  437. w[idx--] = p->pos;
  438. p = p->prev_point;
  439. }
  440. w[0] = p->pos;
  441. }
  442. return path;
  443. }
  444. TypedArray<Vector2i> AStarGrid2D::get_id_path(const Vector2i &p_from_id, const Vector2i &p_to_id) {
  445. ERR_FAIL_COND_V_MSG(dirty, TypedArray<Vector2i>(), "Grid is not initialized. Call the update method.");
  446. ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_from_id), TypedArray<Vector2i>(), vformat("Can't get id path. Point out of bounds (%s/%s, %s/%s)", p_from_id.x, size.width, p_from_id.y, size.height));
  447. ERR_FAIL_COND_V_MSG(!is_in_boundsv(p_to_id), TypedArray<Vector2i>(), vformat("Can't get id path. Point out of bounds (%s/%s, %s/%s)", p_to_id.x, size.width, p_to_id.y, size.height));
  448. Point *a = _get_point(p_from_id.x, p_from_id.y);
  449. Point *b = _get_point(p_to_id.x, p_to_id.y);
  450. if (a == b) {
  451. TypedArray<Vector2i> ret;
  452. ret.push_back(a);
  453. return ret;
  454. }
  455. Point *begin_point = a;
  456. Point *end_point = b;
  457. bool found_route = _solve(begin_point, end_point);
  458. if (!found_route) {
  459. return TypedArray<Vector2i>();
  460. }
  461. Point *p = end_point;
  462. int64_t pc = 1;
  463. while (p != begin_point) {
  464. pc++;
  465. p = p->prev_point;
  466. }
  467. TypedArray<Vector2i> path;
  468. path.resize(pc);
  469. {
  470. p = end_point;
  471. int64_t idx = pc - 1;
  472. while (p != begin_point) {
  473. path[idx--] = p->id;
  474. p = p->prev_point;
  475. }
  476. path[0] = p->id;
  477. }
  478. return path;
  479. }
  480. void AStarGrid2D::_bind_methods() {
  481. ClassDB::bind_method(D_METHOD("set_size", "size"), &AStarGrid2D::set_size);
  482. ClassDB::bind_method(D_METHOD("get_size"), &AStarGrid2D::get_size);
  483. ClassDB::bind_method(D_METHOD("set_offset", "offset"), &AStarGrid2D::set_offset);
  484. ClassDB::bind_method(D_METHOD("get_offset"), &AStarGrid2D::get_offset);
  485. ClassDB::bind_method(D_METHOD("set_cell_size", "cell_size"), &AStarGrid2D::set_cell_size);
  486. ClassDB::bind_method(D_METHOD("get_cell_size"), &AStarGrid2D::get_cell_size);
  487. ClassDB::bind_method(D_METHOD("is_in_bounds", "x", "y"), &AStarGrid2D::is_in_bounds);
  488. ClassDB::bind_method(D_METHOD("is_in_boundsv", "id"), &AStarGrid2D::is_in_boundsv);
  489. ClassDB::bind_method(D_METHOD("is_dirty"), &AStarGrid2D::is_dirty);
  490. ClassDB::bind_method(D_METHOD("update"), &AStarGrid2D::update);
  491. ClassDB::bind_method(D_METHOD("set_jumping_enabled", "enabled"), &AStarGrid2D::set_jumping_enabled);
  492. ClassDB::bind_method(D_METHOD("is_jumping_enabled"), &AStarGrid2D::is_jumping_enabled);
  493. ClassDB::bind_method(D_METHOD("set_diagonal_mode", "mode"), &AStarGrid2D::set_diagonal_mode);
  494. ClassDB::bind_method(D_METHOD("get_diagonal_mode"), &AStarGrid2D::get_diagonal_mode);
  495. ClassDB::bind_method(D_METHOD("set_default_heuristic", "heuristic"), &AStarGrid2D::set_default_heuristic);
  496. ClassDB::bind_method(D_METHOD("get_default_heuristic"), &AStarGrid2D::get_default_heuristic);
  497. ClassDB::bind_method(D_METHOD("set_point_solid", "id", "solid"), &AStarGrid2D::set_point_solid, DEFVAL(true));
  498. ClassDB::bind_method(D_METHOD("is_point_solid", "id"), &AStarGrid2D::is_point_solid);
  499. ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStarGrid2D::set_point_weight_scale);
  500. ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStarGrid2D::get_point_weight_scale);
  501. ClassDB::bind_method(D_METHOD("clear"), &AStarGrid2D::clear);
  502. ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStarGrid2D::get_point_path);
  503. ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStarGrid2D::get_id_path);
  504. GDVIRTUAL_BIND(_estimate_cost, "from_id", "to_id")
  505. GDVIRTUAL_BIND(_compute_cost, "from_id", "to_id")
  506. ADD_PROPERTY(PropertyInfo(Variant::VECTOR2I, "size"), "set_size", "get_size");
  507. ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "offset"), "set_offset", "get_offset");
  508. ADD_PROPERTY(PropertyInfo(Variant::VECTOR2, "cell_size"), "set_cell_size", "get_cell_size");
  509. ADD_PROPERTY(PropertyInfo(Variant::BOOL, "jumping_enabled"), "set_jumping_enabled", "is_jumping_enabled");
  510. ADD_PROPERTY(PropertyInfo(Variant::INT, "default_heuristic", PROPERTY_HINT_ENUM, "Euclidean,Manhattan,Octile,Chebyshev,Max"), "set_default_heuristic", "get_default_heuristic");
  511. ADD_PROPERTY(PropertyInfo(Variant::INT, "diagonal_mode", PROPERTY_HINT_ENUM, "Never,Always,At Least One Walkable,Only If No Obstacles,Max"), "set_diagonal_mode", "get_diagonal_mode");
  512. BIND_ENUM_CONSTANT(HEURISTIC_EUCLIDEAN);
  513. BIND_ENUM_CONSTANT(HEURISTIC_MANHATTAN);
  514. BIND_ENUM_CONSTANT(HEURISTIC_OCTILE);
  515. BIND_ENUM_CONSTANT(HEURISTIC_CHEBYSHEV);
  516. BIND_ENUM_CONSTANT(HEURISTIC_MAX);
  517. BIND_ENUM_CONSTANT(DIAGONAL_MODE_ALWAYS);
  518. BIND_ENUM_CONSTANT(DIAGONAL_MODE_NEVER);
  519. BIND_ENUM_CONSTANT(DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE);
  520. BIND_ENUM_CONSTANT(DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES);
  521. BIND_ENUM_CONSTANT(DIAGONAL_MODE_MAX);
  522. }