aboutsummaryrefslogtreecommitdiff
path: root/core/math/a_star.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'core/math/a_star.cpp')
-rw-r--r--core/math/a_star.cpp72
1 files changed, 57 insertions, 15 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index ffe108996..7e26761ab 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -42,8 +42,10 @@ int AStar::get_available_point_id() const {
}
void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) {
+
ERR_FAIL_COND(p_id < 0);
ERR_FAIL_COND(p_weight_scale < 1);
+
if (!points.has(p_id)) {
Point *pt = memnew(Point);
pt->id = p_id;
@@ -64,12 +66,29 @@ Vector3 AStar::get_point_position(int p_id) const {
return points[p_id]->pos;
}
+
+void AStar::set_point_position(int p_id, const Vector3 &p_pos) {
+
+ ERR_FAIL_COND(!points.has(p_id));
+
+ points[p_id]->pos = p_pos;
+}
+
real_t AStar::get_point_weight_scale(int p_id) const {
ERR_FAIL_COND_V(!points.has(p_id), 0);
return points[p_id]->weight_scale;
}
+
+void AStar::set_point_weight_scale(int p_id, real_t p_weight_scale) {
+
+ ERR_FAIL_COND(!points.has(p_id));
+ ERR_FAIL_COND(p_weight_scale < 1);
+
+ points[p_id]->weight_scale = p_weight_scale;
+}
+
void AStar::remove_point(int p_id) {
ERR_FAIL_COND(!points.has(p_id));
@@ -130,6 +149,7 @@ bool AStar::has_point(int p_id) const {
}
Array AStar::get_points() {
+
Array point_list;
for (const Map<int, Point *>::Element *E = points.front(); E; E = E->next()) {
@@ -139,6 +159,21 @@ Array AStar::get_points() {
return point_list;
}
+PoolVector<int> AStar::get_point_connections(int p_id) {
+
+ ERR_FAIL_COND_V(!points.has(p_id), PoolVector<int>());
+
+ PoolVector<int> point_list;
+
+ Point *p = points[p_id];
+
+ for (int i = 0; i < p->neighbours.size(); i++) {
+ point_list.push_back(p->neighbours[i]->id);
+ }
+
+ return point_list;
+}
+
bool AStar::are_points_connected(int p_id, int p_with_id) const {
Segment s(p_id, p_with_id);
@@ -171,6 +206,7 @@ int AStar::get_closest_point(const Vector3 &p_point) const {
return closest_id;
}
+
Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const {
real_t closest_dist = 1e20;
@@ -222,15 +258,15 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
while (!found_route) {
if (open_list.first() == NULL) {
- //could not find path sadly
+ // No path found
break;
}
- //check open list
+ // Check open list
SelfList<Point> *least_cost_point = NULL;
real_t least_cost = 1e30;
- //this could be faster (cache previous results)
+ // TODO: Cache previous results
for (SelfList<Point> *E = open_list.first(); E; E = E->next()) {
Point *p = E->self();
@@ -246,7 +282,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
}
Point *p = least_cost_point->self();
- //open the neighbours for search
+ // Open the neighbours for search
int es = p->neighbours.size();
for (int i = 0; i < es; i++) {
@@ -256,7 +292,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
real_t distance = _compute_cost(p->id, e->id) * e->weight_scale + p->distance;
if (e->last_pass == pass) {
- //oh this was visited already, can we win the cost?
+ // Already visited, is this cheaper?
if (e->distance > distance) {
@@ -264,15 +300,15 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
e->distance = distance;
}
} else {
- //add to open neighbours
+ // Add to open neighbours
e->prev_point = p;
e->distance = distance;
- e->last_pass = pass; //mark as used
+ e->last_pass = pass; // Mark as used
open_list.add(&e->list);
if (e == end_point) {
- //oh my reached end! stop algorithm
+ // End reached; stop algorithm
found_route = true;
break;
}
@@ -285,7 +321,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
open_list.remove(least_cost_point);
}
- //clear the openf list
+ // Clear the openf list
while (open_list.first()) {
open_list.remove(open_list.first());
}
@@ -294,6 +330,7 @@ bool AStar::_solve(Point *begin_point, Point *end_point) {
}
float AStar::_estimate_cost(int p_from_id, int p_to_id) {
+
if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_estimate_cost))
return get_script_instance()->call(SceneStringNames::get_singleton()->_estimate_cost, p_from_id, p_to_id);
@@ -301,6 +338,7 @@ float AStar::_estimate_cost(int p_from_id, int p_to_id) {
}
float AStar::_compute_cost(int p_from_id, int p_to_id) {
+
if (get_script_instance() && get_script_instance()->has_method(SceneStringNames::get_singleton()->_compute_cost))
return get_script_instance()->call(SceneStringNames::get_singleton()->_compute_cost, p_from_id, p_to_id);
@@ -331,9 +369,9 @@ PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) {
if (!found_route)
return PoolVector<Vector3>();
- //midpoints
+ // Midpoints
Point *p = end_point;
- int pc = 1; //begin point
+ int pc = 1; // Begin point
while (p != begin_point) {
pc++;
p = p->prev_point;
@@ -352,7 +390,7 @@ PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) {
p = p->prev_point;
}
- w[0] = p->pos; //assign first
+ w[0] = p->pos; // Assign first
}
return path;
@@ -382,9 +420,9 @@ PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) {
if (!found_route)
return PoolVector<int>();
- //midpoints
+ // Midpoints
Point *p = end_point;
- int pc = 1; //begin point
+ int pc = 1; // Begin point
while (p != begin_point) {
pc++;
p = p->prev_point;
@@ -403,7 +441,7 @@ PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) {
p = p->prev_point;
}
- w[0] = p->id; //assign first
+ w[0] = p->id; // Assign first
}
return path;
@@ -414,11 +452,15 @@ void AStar::_bind_methods() {
ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar::get_available_point_id);
ClassDB::bind_method(D_METHOD("add_point", "id", "position", "weight_scale"), &AStar::add_point, DEFVAL(1.0));
ClassDB::bind_method(D_METHOD("get_point_position", "id"), &AStar::get_point_position);
+ ClassDB::bind_method(D_METHOD("set_point_position", "id", "position"), &AStar::set_point_position);
ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStar::get_point_weight_scale);
+ ClassDB::bind_method(D_METHOD("set_point_weight_scale", "id", "weight_scale"), &AStar::set_point_weight_scale);
ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar::remove_point);
ClassDB::bind_method(D_METHOD("has_point", "id"), &AStar::has_point);
ClassDB::bind_method(D_METHOD("get_points"), &AStar::get_points);
+ ClassDB::bind_method(D_METHOD("get_point_connections"), &AStar::get_point_connections);
+
ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar::connect_points, DEFVAL(true));
ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id"), &AStar::disconnect_points);
ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id"), &AStar::are_points_connected);
generated by cgit v1.2.3-70-g09d2 (git 2.52.0) at 2025-12-12 14:58:07 +0000