Skip to content

Commit 202e197

Browse files
10Drenth10Drenth
committed
Improve AStarGrid2D performance when jumping is enabled
1 parent 8120e03 commit 202e197

File tree

2 files changed

+132
-77
lines changed

2 files changed

+132
-77
lines changed

‎core/math/a_star_grid_2d.cpp‎

Lines changed: 113 additions & 72 deletions
Original file line numberDiff line numberDiff line change
@@ -127,13 +127,18 @@ void AStarGrid2D::update() {
127127
}
128128

129129
points.clear();
130+
solid_mask.clear();
130131

131132
const int32_t end_x = region.get_end().x;
132133
const int32_t end_y = region.get_end().y;
133134
const Vector2 half_cell_size = cell_size / 2;
135+
for (int32_t x = region.position.x; x < end_x + 2; x++) {
136+
solid_mask.push_back(true);
137+
}
134138

135139
for (int32_t y = region.position.y; y < end_y; y++) {
136140
LocalVector<Point> line;
141+
solid_mask.push_back(true);
137142
for (int32_t x = region.position.x; x < end_x; x++) {
138143
Vector2 v = offset;
139144
switch (cell_shape) {
@@ -150,10 +155,16 @@ void AStarGrid2D::update() {
150155
break;
151156
}
152157
line.push_back(Point(Vector2i(x, y), v));
158+
solid_mask.push_back(false);
153159
}
160+
solid_mask.push_back(true);
154161
points.push_back(line);
155162
}
156163

164+
for (int32_t x = region.position.x; x < end_x + 2; x++) {
165+
solid_mask.push_back(true);
166+
}
167+
157168
dirty = false;
158169
}
159170

@@ -207,13 +218,13 @@ AStarGrid2D::Heuristic AStarGrid2D::get_default_estimate_heuristic() const {
207218
void AStarGrid2D::set_point_solid(const Vector2i &p_id, bool p_solid) {
208219
ERR_FAIL_COND_MSG(dirty, "Grid is not initialized. Call the update method.");
209220
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));
210-
_get_point_unchecked(p_id)->solid = p_solid;
221+
_set_solid_unchecked(p_id, p_solid);
211222
}
212223

213224
bool AStarGrid2D::is_point_solid(const Vector2i &p_id) const {
214225
ERR_FAIL_COND_V_MSG(dirty, false, "Grid is not initialized. Call the update method.");
215226
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));
216-
return _get_point_unchecked(p_id)->solid;
227+
return _get_solid_unchecked(p_id);
217228
}
218229

219230
void AStarGrid2D::set_point_weight_scale(const Vector2i &p_id, real_t p_weight_scale) {
@@ -238,7 +249,7 @@ void AStarGrid2D::fill_solid_region(const Rect2i &p_region, bool p_solid) {
238249

239250
for (int32_t y = safe_region.position.y; y < end_y; y++) {
240251
for (int32_t x = safe_region.position.x; x < end_x; x++) {
241-
_get_point_unchecked(x, y)->solid = p_solid;
252+
_set_solid_unchecked(x, y, p_solid);
242253
}
243254
}
244255
}
@@ -259,13 +270,6 @@ void AStarGrid2D::fill_weight_scale_region(const Rect2i &p_region, real_t p_weig
259270
}
260271

261272
AStarGrid2D::Point *AStarGrid2D::_jump(Point *p_from, Point *p_to) {
262-
if (!p_to || p_to->solid) {
263-
return nullptr;
264-
}
265-
if (p_to == end) {
266-
return p_to;
267-
}
268-
269273
int32_t from_x = p_from->id.x;
270274
int32_t from_y = p_from->id.y;
271275

@@ -276,72 +280,109 @@ AStarGrid2D::Point *AStarGrid2D::_jump(Point *p_from, Point *p_to) {
276280
int32_t dy = to_y - from_y;
277281

278282
if (diagonal_mode == DIAGONAL_MODE_ALWAYS || diagonal_mode == DIAGONAL_MODE_AT_LEAST_ONE_WALKABLE) {
279-
if (dx != 0 && dy != 0) {
280-
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))) {
281-
return p_to;
282-
}
283-
if (_jump(p_to, _get_point(to_x + dx, to_y)) != nullptr) {
284-
return p_to;
283+
if (dx == 0 || dy == 0) {
284+
return _forced_successor(to_x, to_y, dx, dy);
285+
}
286+
287+
while (_is_walkable(to_x, to_y) && (diagonal_mode == DIAGONAL_MODE_ALWAYS || _is_walkable(to_x, to_y - dy) || _is_walkable(to_x - dx, to_y))) {
288+
if (end->id.x == to_x && end->id.y == to_y) {
289+
return end;
285290
}
286-
if (_jump(p_to, _get_point(to_x, to_y + dy)) != nullptr) {
287-
return p_to;
291+
292+
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))) {
293+
return _get_point_unchecked(to_x, to_y);
288294
}
289-
} else {
290-
if (dx != 0) {
291-
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))) {
292-
return p_to;
293-
}
294-
} else {
295-
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))) {
296-
return p_to;
297-
}
295+
296+
if (_forced_successor(to_x + dx, to_y, dx, 0) != nullptr || _forced_successor(to_x, to_y + dy, 0, dy) != nullptr) {
297+
return _get_point_unchecked(to_x, to_y);
298298
}
299+
300+
to_x += dx;
301+
to_y += dy;
299302
}
300-
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)))) {
301-
return _jump(p_to, _get_point(to_x + dx, to_y + dy));
302-
}
303+
303304
} else if (diagonal_mode == DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES) {
304-
if (dx != 0 && dy != 0) {
305-
if ((_is_walkable(to_x + dx, to_y + dy) && !_is_walkable(to_x, to_y + dy)) || !_is_walkable(to_x + dx, to_y)) {
306-
return p_to;
307-
}
308-
if (_jump(p_to, _get_point(to_x + dx, to_y)) != nullptr) {
309-
return p_to;
305+
if (dx == 0 || dy == 0) {
306+
return _forced_successor(from_x, from_y, dx, dy, true);
307+
}
308+
309+
while (_is_walkable(to_x, to_y) && _is_walkable(to_x, to_y - dy) && _is_walkable(to_x - dx, to_y)) {
310+
if (end->id.x == to_x && end->id.y == to_y) {
311+
return end;
310312
}
311-
if (_jump(p_to, _get_point(to_x, to_y + dy)) != nullptr) {
312-
return p_to;
313+
314+
if ((_is_walkable(to_x + dx, to_y + dy) && !_is_walkable(to_x, to_y + dy)) || !_is_walkable(to_x + dx, to_y)) {
315+
return _get_point_unchecked(to_x, to_y);
313316
}
314-
} else {
315-
if (dx != 0) {
316-
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))) {
317-
return p_to;
318-
}
319-
} else {
320-
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))) {
321-
return p_to;
322-
}
317+
318+
if (_forced_successor(to_x, to_y, dx, 0) != nullptr || _forced_successor(to_x, to_y, 0, dy) != nullptr) {
319+
return _get_point_unchecked(to_x, to_y);
323320
}
321+
322+
to_x += dx;
323+
to_y += dy;
324324
}
325-
if (_is_walkable(to_x + dx, to_y + dy) && _is_walkable(to_x + dx, to_y) && _is_walkable(to_x, to_y + dy)) {
326-
return _jump(p_to, _get_point(to_x + dx, to_y + dy));
327-
}
325+
328326
} else { // DIAGONAL_MODE_NEVER
329-
if (dx != 0) {
330-
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))) {
331-
return p_to;
327+
if (dy == 0) {
328+
return _forced_successor(from_x, from_y, dx, 0, true);
329+
}
330+
331+
while (_is_walkable(to_x, to_y)) {
332+
if (end->id.x == to_x && end->id.y == to_y) {
333+
return end;
332334
}
333-
} else if (dy != 0) {
335+
334336
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))) {
335-
return p_to;
336-
}
337-
if (_jump(p_to, _get_point(to_x + 1, to_y)) != nullptr) {
338-
return p_to;
337+
return _get_point_unchecked(to_x, to_y);
339338
}
340-
if (_jump(p_to, _get_point(to_x - 1, to_y)) != nullptr) {
341-
return p_to;
339+
340+
if (_forced_successor(to_x, to_y, 1, 0, true) != nullptr || _forced_successor(to_x, to_y, -1, 0, true) != nullptr) {
341+
return _get_point_unchecked(to_x, to_y);
342342
}
343+
344+
to_y += dy;
343345
}
344-
return _jump(p_to, _get_point(to_x + dx, to_y + dy));
346+
}
347+
348+
return nullptr;
349+
}
350+
351+
AStarGrid2D::Point *AStarGrid2D::_forced_successor(int32_t p_x, int32_t p_y, int32_t p_dx, int32_t p_dy, bool p_inclusive) {
352+
// Remembering previous results can improve performance.
353+
bool l_prev = false, r_prev = false, l = false, r = false;
354+
355+
int32_t o_x = p_x, o_y = p_y;
356+
if (p_inclusive) {
357+
o_x += p_dx;
358+
o_y += p_dy;
359+
}
360+
361+
int32_t l_x = p_x - p_dy, l_y = p_y - p_dx;
362+
int32_t r_x = p_x + p_dy, r_y = p_y + p_dx;
363+
364+
while (_is_walkable(o_x, o_y)) {
365+
if (end->id.x == o_x && end->id.y == o_y) {
366+
return end;
367+
}
368+
369+
l_prev = l || _is_walkable(l_x, l_y);
370+
r_prev = r || _is_walkable(r_x, r_y);
371+
372+
l_x += p_dx;
373+
l_y += p_dy;
374+
r_x += p_dx;
375+
r_y += p_dy;
376+
377+
l = _is_walkable(l_x, l_y);
378+
r = _is_walkable(r_x, r_y);
379+
380+
if ((l && !l_prev) || (r && !r_prev)) {
381+
return _get_point_unchecked(o_x, o_y);
382+
}
383+
384+
o_x += p_dx;
385+
o_y += p_dy;
345386
}
346387
return nullptr;
347388
}
@@ -394,19 +435,19 @@ void AStarGrid2D::_get_nbors(Point *p_point, LocalVector<Point *> &r_nbors) {
394435
}
395436
}
396437

397-
if (top && !top->solid) {
438+
if (top && !_get_solid_unchecked(top->id)) {
398439
r_nbors.push_back(top);
399440
ts0 = true;
400441
}
401-
if (right && !right->solid) {
442+
if (right && !_get_solid_unchecked(right->id)) {
402443
r_nbors.push_back(right);
403444
ts1 = true;
404445
}
405-
if (bottom && !bottom->solid) {
446+
if (bottom && !_get_solid_unchecked(bottom->id)) {
406447
r_nbors.push_back(bottom);
407448
ts2 = true;
408449
}
409-
if (left && !left->solid) {
450+
if (left && !_get_solid_unchecked(left->id)) {
410451
r_nbors.push_back(left);
411452
ts3 = true;
412453
}
@@ -436,16 +477,16 @@ void AStarGrid2D::_get_nbors(Point *p_point, LocalVector<Point *> &r_nbors) {
436477
break;
437478
}
438479

439-
if (td0 && (top_left && !top_left->solid)) {
480+
if (td0 && (top_left && !_get_solid_unchecked(top_left->id))) {
440481
r_nbors.push_back(top_left);
441482
}
442-
if (td1 && (top_right && !top_right->solid)) {
483+
if (td1 && (top_right && !_get_solid_unchecked(top_right->id))) {
443484
r_nbors.push_back(top_right);
444485
}
445-
if (td2 && (bottom_right && !bottom_right->solid)) {
486+
if (td2 && (bottom_right && !_get_solid_unchecked(bottom_right->id))) {
446487
r_nbors.push_back(bottom_right);
447488
}
448-
if (td3 && (bottom_left && !bottom_left->solid)) {
489+
if (td3 && (bottom_left && !_get_solid_unchecked(bottom_left->id))) {
449490
r_nbors.push_back(bottom_left);
450491
}
451492
}
@@ -454,7 +495,7 @@ bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
454495
last_closest_point = nullptr;
455496
pass++;
456497

457-
if (p_end_point->solid) {
498+
if (_get_solid_unchecked(p_end_point->id)) {
458499
return false;
459500
}
460501

@@ -500,7 +541,7 @@ bool AStarGrid2D::_solve(Point *p_begin_point, Point *p_end_point) {
500541
continue;
501542
}
502543
} else {
503-
if (e->solid || e->closed_pass == pass) {
544+
if (_get_solid_unchecked(e->id) || e->closed_pass == pass) {
504545
continue;
505546
}
506547
weight_scale = e->weight_scale;
@@ -580,7 +621,7 @@ TypedArray<Dictionary> AStarGrid2D::get_point_data_in_region(const Rect2i &p_reg
580621
Dictionary dict;
581622
dict["id"] = p.id;
582623
dict["position"] = p.pos;
583-
dict["solid"] = p.solid;
624+
dict["solid"] = _get_solid_unchecked(p.id);
584625
dict["weight_scale"] = p.weight_scale;
585626
data.push_back(dict);
586627
}

‎core/math/a_star_grid_2d.h‎

Lines changed: 19 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,6 @@ class AStarGrid2D : public RefCounted {
7878
struct Point {
7979
Vector2i id;
8080

81-
bool solid = false;
8281
Vector2 pos;
8382
real_t weight_scale = 1.0;
8483

@@ -111,18 +110,20 @@ class AStarGrid2D : public RefCounted {
111110
}
112111
};
113112

113+
LocalVector<bool> solid_mask;
114114
LocalVector<LocalVector<Point>> points;
115115
Point *end = nullptr;
116116
Point *last_closest_point = nullptr;
117117

118118
uint64_t pass = 1;
119119

120120
private: // Internal routines.
121+
_FORCE_INLINE_ size_t _to_mask_index(int32_t p_x, int32_t p_y) const {
122+
return ((p_y - region.position.y + 1) * (region.size.x + 2)) + p_x - region.position.x + 1;
123+
}
124+
121125
_FORCE_INLINE_ bool _is_walkable(int32_t p_x, int32_t p_y) const {
122-
if (region.has_point(Vector2i(p_x, p_y))) {
123-
return !points[p_y - region.position.y][p_x - region.position.x].solid;
124-
}
125-
return false;
126+
return !solid_mask[_to_mask_index(p_x, p_y)];
126127
}
127128

128129
_FORCE_INLINE_ Point *_get_point(int32_t p_x, int32_t p_y) {
@@ -132,6 +133,18 @@ class AStarGrid2D : public RefCounted {
132133
return nullptr;
133134
}
134135

136+
_FORCE_INLINE_ void _set_solid_unchecked(int32_t p_x, int32_t p_y, bool p_solid) {
137+
solid_mask[_to_mask_index(p_x, p_y)] = p_solid;
138+
}
139+
140+
_FORCE_INLINE_ void _set_solid_unchecked(const Vector2i &p_id, bool p_solid) {
141+
solid_mask[_to_mask_index(p_id.x, p_id.y)] = p_solid;
142+
}
143+
144+
_FORCE_INLINE_ bool _get_solid_unchecked(const Vector2i &p_id) const {
145+
return solid_mask[_to_mask_index(p_id.x, p_id.y)];
146+
}
147+
135148
_FORCE_INLINE_ Point *_get_point_unchecked(int32_t p_x, int32_t p_y) {
136149
return &points[p_y - region.position.y][p_x - region.position.x];
137150
}
@@ -146,6 +159,7 @@ class AStarGrid2D : public RefCounted {
146159

147160
void _get_nbors(Point *p_point, LocalVector<Point *> &r_nbors);
148161
Point *_jump(Point *p_from, Point *p_to);
162+
Point *_forced_successor(int32_t p_x, int32_t p_y, int32_t p_dx, int32_t p_dy, bool p_inclusive = false);
149163
bool _solve(Point *p_begin_point, Point *p_end_point);
150164

151165
protected:

0 commit comments

Comments
 (0)