|
@@ -49,10 +49,9 @@ void AStar3D::add_point(int64_t p_id, const Vector3 &p_pos, real_t p_weight_scal
|
|
|
ERR_FAIL_COND_MSG(p_id < 0, vformat("Can't add a point with negative id: %d.", p_id));
|
|
|
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));
|
|
|
|
|
|
- Point *found_pt;
|
|
|
- bool p_exists = points.lookup(p_id, found_pt);
|
|
|
+ Point **point_entry = points.getptr(p_id);
|
|
|
|
|
|
- if (!p_exists) {
|
|
|
+ if (!point_entry) {
|
|
|
Point *pt = memnew(Point);
|
|
|
pt->id = p_id;
|
|
|
pt->pos = p_pos;
|
|
@@ -61,89 +60,86 @@ void AStar3D::add_point(int64_t p_id, const Vector3 &p_pos, real_t p_weight_scal
|
|
|
pt->open_pass = 0;
|
|
|
pt->closed_pass = 0;
|
|
|
pt->enabled = true;
|
|
|
- points.set(p_id, pt);
|
|
|
+ points.insert_new(p_id, pt);
|
|
|
} else {
|
|
|
+ Point *found_pt = *point_entry;
|
|
|
found_pt->pos = p_pos;
|
|
|
found_pt->weight_scale = p_weight_scale;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
Vector3 AStar3D::get_point_position(int64_t p_id) const {
|
|
|
- Point *p = nullptr;
|
|
|
- bool p_exists = points.lookup(p_id, p);
|
|
|
- ERR_FAIL_COND_V_MSG(!p_exists, Vector3(), vformat("Can't get point's position. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point *const *point_entry = points.getptr(p_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!point_entry, Vector3(), vformat("Can't get point's position. Point with id: %d doesn't exist.", p_id));
|
|
|
|
|
|
- return p->pos;
|
|
|
+ return (*point_entry)->pos;
|
|
|
}
|
|
|
|
|
|
void AStar3D::set_point_position(int64_t p_id, const Vector3 &p_pos) {
|
|
|
- Point *p = nullptr;
|
|
|
- bool p_exists = points.lookup(p_id, p);
|
|
|
- ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set point's position. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point **point_entry = points.getptr(p_id);
|
|
|
+ ERR_FAIL_COND_MSG(!point_entry, vformat("Can't set point's position. Point with id: %d doesn't exist.", p_id));
|
|
|
|
|
|
- p->pos = p_pos;
|
|
|
+ (*point_entry)->pos = p_pos;
|
|
|
}
|
|
|
|
|
|
real_t AStar3D::get_point_weight_scale(int64_t p_id) const {
|
|
|
- Point *p = nullptr;
|
|
|
- bool p_exists = points.lookup(p_id, p);
|
|
|
- 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));
|
|
|
+ Point *const *point_entry = points.getptr(p_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!point_entry, 0, vformat("Can't get point's weight scale. Point with id: %d doesn't exist.", p_id));
|
|
|
|
|
|
- return p->weight_scale;
|
|
|
+ return (*point_entry)->weight_scale;
|
|
|
}
|
|
|
|
|
|
void AStar3D::set_point_weight_scale(int64_t p_id, real_t p_weight_scale) {
|
|
|
- Point *p = nullptr;
|
|
|
- bool p_exists = points.lookup(p_id, p);
|
|
|
- ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set point's weight scale. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point **point_entry = points.getptr(p_id);
|
|
|
+ ERR_FAIL_COND_MSG(!point_entry, vformat("Can't set point's weight scale. Point with id: %d doesn't exist.", p_id));
|
|
|
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));
|
|
|
|
|
|
- p->weight_scale = p_weight_scale;
|
|
|
+ (*point_entry)->weight_scale = p_weight_scale;
|
|
|
}
|
|
|
|
|
|
void AStar3D::remove_point(int64_t p_id) {
|
|
|
- Point *p = nullptr;
|
|
|
- bool p_exists = points.lookup(p_id, p);
|
|
|
- ERR_FAIL_COND_MSG(!p_exists, vformat("Can't remove point. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point **point_entry = points.getptr(p_id);
|
|
|
+ ERR_FAIL_COND_MSG(!point_entry, vformat("Can't remove point. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point *p = *point_entry;
|
|
|
|
|
|
- for (OAHashMap<int64_t, Point *>::Iterator it = p->neighbors.iter(); it.valid; it = p->neighbors.next_iter(it)) {
|
|
|
- Segment s(p_id, (*it.key));
|
|
|
+ for (KeyValue<int64_t, Point *> &kv : p->neighbors) {
|
|
|
+ Segment s(p_id, kv.key);
|
|
|
segments.erase(s);
|
|
|
|
|
|
- (*it.value)->neighbors.remove(p->id);
|
|
|
- (*it.value)->unlinked_neighbours.remove(p->id);
|
|
|
+ kv.value->neighbors.erase(p->id);
|
|
|
+ kv.value->unlinked_neighbours.erase(p->id);
|
|
|
}
|
|
|
|
|
|
- for (OAHashMap<int64_t, Point *>::Iterator it = p->unlinked_neighbours.iter(); it.valid; it = p->unlinked_neighbours.next_iter(it)) {
|
|
|
- Segment s(p_id, (*it.key));
|
|
|
+ for (KeyValue<int64_t, Point *> &kv : p->unlinked_neighbours) {
|
|
|
+ Segment s(p_id, kv.key);
|
|
|
segments.erase(s);
|
|
|
|
|
|
- (*it.value)->neighbors.remove(p->id);
|
|
|
- (*it.value)->unlinked_neighbours.remove(p->id);
|
|
|
+ kv.value->neighbors.erase(p->id);
|
|
|
+ kv.value->unlinked_neighbours.erase(p->id);
|
|
|
}
|
|
|
|
|
|
memdelete(p);
|
|
|
- points.remove(p_id);
|
|
|
+ points.erase(p_id);
|
|
|
last_free_id = p_id;
|
|
|
}
|
|
|
|
|
|
void AStar3D::connect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) {
|
|
|
ERR_FAIL_COND_MSG(p_id == p_with_id, vformat("Can't connect point with id: %d to itself.", p_id));
|
|
|
|
|
|
- Point *a = nullptr;
|
|
|
- bool from_exists = points.lookup(p_id, a);
|
|
|
- ERR_FAIL_COND_MSG(!from_exists, vformat("Can't connect points. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point **a_entry = points.getptr(p_id);
|
|
|
+ ERR_FAIL_COND_MSG(!a_entry, vformat("Can't connect points. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point *a = *a_entry;
|
|
|
|
|
|
- Point *b = nullptr;
|
|
|
- bool to_exists = points.lookup(p_with_id, b);
|
|
|
- ERR_FAIL_COND_MSG(!to_exists, vformat("Can't connect points. Point with id: %d doesn't exist.", p_with_id));
|
|
|
+ Point **b_entry = points.getptr(p_with_id);
|
|
|
+ ERR_FAIL_COND_MSG(!b_entry, vformat("Can't connect points. Point with id: %d doesn't exist.", p_with_id));
|
|
|
+ Point *b = *b_entry;
|
|
|
|
|
|
- a->neighbors.set(b->id, b);
|
|
|
+ a->neighbors.insert(b->id, b);
|
|
|
|
|
|
if (bidirectional) {
|
|
|
- b->neighbors.set(a->id, a);
|
|
|
+ b->neighbors.insert(a->id, a);
|
|
|
} else {
|
|
|
- b->unlinked_neighbours.set(a->id, a);
|
|
|
+ b->unlinked_neighbours.insert(a->id, a);
|
|
|
}
|
|
|
|
|
|
Segment s(p_id, p_with_id);
|
|
@@ -156,8 +152,8 @@ void AStar3D::connect_points(int64_t p_id, int64_t p_with_id, bool bidirectional
|
|
|
s.direction |= element->direction;
|
|
|
if (s.direction == Segment::BIDIRECTIONAL) {
|
|
|
// Both are neighbors of each other now
|
|
|
- a->unlinked_neighbours.remove(b->id);
|
|
|
- b->unlinked_neighbours.remove(a->id);
|
|
|
+ a->unlinked_neighbours.erase(b->id);
|
|
|
+ b->unlinked_neighbours.erase(a->id);
|
|
|
}
|
|
|
segments.remove(element);
|
|
|
}
|
|
@@ -166,13 +162,13 @@ void AStar3D::connect_points(int64_t p_id, int64_t p_with_id, bool bidirectional
|
|
|
}
|
|
|
|
|
|
void AStar3D::disconnect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) {
|
|
|
- Point *a = nullptr;
|
|
|
- bool a_exists = points.lookup(p_id, a);
|
|
|
- ERR_FAIL_COND_MSG(!a_exists, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point **a_entry = points.getptr(p_id);
|
|
|
+ ERR_FAIL_COND_MSG(!a_entry, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point *a = *a_entry;
|
|
|
|
|
|
- Point *b = nullptr;
|
|
|
- bool b_exists = points.lookup(p_with_id, b);
|
|
|
- ERR_FAIL_COND_MSG(!b_exists, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_with_id));
|
|
|
+ Point **b_entry = points.getptr(p_with_id);
|
|
|
+ ERR_FAIL_COND_MSG(!b_entry, vformat("Can't disconnect points. Point with id: %d doesn't exist.", p_with_id));
|
|
|
+ Point *b = *b_entry;
|
|
|
|
|
|
Segment s(p_id, p_with_id);
|
|
|
int remove_direction = bidirectional ? (int)Segment::BIDIRECTIONAL : (int)s.direction;
|
|
@@ -183,18 +179,18 @@ void AStar3D::disconnect_points(int64_t p_id, int64_t p_with_id, bool bidirectio
|
|
|
// Erase the directions to be removed
|
|
|
s.direction = (element->direction & ~remove_direction);
|
|
|
|
|
|
- a->neighbors.remove(b->id);
|
|
|
+ a->neighbors.erase(b->id);
|
|
|
if (bidirectional) {
|
|
|
- b->neighbors.remove(a->id);
|
|
|
+ b->neighbors.erase(a->id);
|
|
|
if (element->direction != Segment::BIDIRECTIONAL) {
|
|
|
- a->unlinked_neighbours.remove(b->id);
|
|
|
- b->unlinked_neighbours.remove(a->id);
|
|
|
+ a->unlinked_neighbours.erase(b->id);
|
|
|
+ b->unlinked_neighbours.erase(a->id);
|
|
|
}
|
|
|
} else {
|
|
|
if (s.direction == Segment::NONE) {
|
|
|
- b->unlinked_neighbours.remove(a->id);
|
|
|
+ b->unlinked_neighbours.erase(a->id);
|
|
|
} else {
|
|
|
- a->unlinked_neighbours.set(b->id, b);
|
|
|
+ a->unlinked_neighbours.insert(b->id, b);
|
|
|
}
|
|
|
}
|
|
|
|
|
@@ -212,22 +208,21 @@ bool AStar3D::has_point(int64_t p_id) const {
|
|
|
PackedInt64Array AStar3D::get_point_ids() {
|
|
|
PackedInt64Array point_list;
|
|
|
|
|
|
- for (OAHashMap<int64_t, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) {
|
|
|
- point_list.push_back(*(it.key));
|
|
|
+ for (KeyValue<int64_t, Point *> &kv : points) {
|
|
|
+ point_list.push_back(kv.key);
|
|
|
}
|
|
|
|
|
|
return point_list;
|
|
|
}
|
|
|
|
|
|
Vector<int64_t> AStar3D::get_point_connections(int64_t p_id) {
|
|
|
- Point *p = nullptr;
|
|
|
- bool p_exists = points.lookup(p_id, p);
|
|
|
- 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));
|
|
|
+ Point **p_entry = points.getptr(p_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!p_entry, Vector<int64_t>(), vformat("Can't get point's connections. Point with id: %d doesn't exist.", p_id));
|
|
|
|
|
|
Vector<int64_t> point_list;
|
|
|
|
|
|
- for (OAHashMap<int64_t, Point *>::Iterator it = p->neighbors.iter(); it.valid; it = p->neighbors.next_iter(it)) {
|
|
|
- point_list.push_back((*it.key));
|
|
|
+ for (KeyValue<int64_t, Point *> &kv : (*p_entry)->neighbors) {
|
|
|
+ point_list.push_back(kv.key);
|
|
|
}
|
|
|
|
|
|
return point_list;
|
|
@@ -243,15 +238,15 @@ bool AStar3D::are_points_connected(int64_t p_id, int64_t p_with_id, bool bidirec
|
|
|
|
|
|
void AStar3D::clear() {
|
|
|
last_free_id = 0;
|
|
|
- for (OAHashMap<int64_t, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) {
|
|
|
- memdelete(*(it.value));
|
|
|
+ for (KeyValue<int64_t, Point *> &kv : points) {
|
|
|
+ memdelete(kv.value);
|
|
|
}
|
|
|
segments.clear();
|
|
|
points.clear();
|
|
|
}
|
|
|
|
|
|
int64_t AStar3D::get_point_count() const {
|
|
|
- return points.get_num_elements();
|
|
|
+ return points.size();
|
|
|
}
|
|
|
|
|
|
int64_t AStar3D::get_point_capacity() const {
|
|
@@ -267,15 +262,15 @@ int64_t AStar3D::get_closest_point(const Vector3 &p_point, bool p_include_disabl
|
|
|
int64_t closest_id = -1;
|
|
|
real_t closest_dist = 1e20;
|
|
|
|
|
|
- for (OAHashMap<int64_t, Point *>::Iterator it = points.iter(); it.valid; it = points.next_iter(it)) {
|
|
|
- if (!p_include_disabled && !(*it.value)->enabled) {
|
|
|
+ for (const KeyValue<int64_t, Point *> &kv : points) {
|
|
|
+ if (!p_include_disabled && !kv.value->enabled) {
|
|
|
continue; // Disabled points should not be considered.
|
|
|
}
|
|
|
|
|
|
// Keep the closest point's ID, and in case of multiple closest IDs,
|
|
|
// the smallest one (makes it deterministic).
|
|
|
- real_t d = p_point.distance_squared_to((*it.value)->pos);
|
|
|
- int64_t id = *(it.key);
|
|
|
+ real_t d = p_point.distance_squared_to(kv.value->pos);
|
|
|
+ int64_t id = kv.key;
|
|
|
if (d <= closest_dist) {
|
|
|
if (d == closest_dist && id > closest_id) { // Keep lowest ID.
|
|
|
continue;
|
|
@@ -293,9 +288,8 @@ Vector3 AStar3D::get_closest_position_in_segment(const Vector3 &p_point) const {
|
|
|
Vector3 closest_point;
|
|
|
|
|
|
for (const Segment &E : segments) {
|
|
|
- Point *from_point = nullptr, *to_point = nullptr;
|
|
|
- points.lookup(E.key.first, from_point);
|
|
|
- points.lookup(E.key.second, to_point);
|
|
|
+ const Point *from_point = *points.getptr(E.key.first);
|
|
|
+ const Point *to_point = *points.getptr(E.key.second);
|
|
|
|
|
|
if (!(from_point->enabled && to_point->enabled)) {
|
|
|
continue;
|
|
@@ -348,8 +342,8 @@ bool AStar3D::_solve(Point *begin_point, Point *end_point, bool p_allow_partial_
|
|
|
open_list.remove_at(open_list.size() - 1);
|
|
|
p->closed_pass = pass; // Mark the point as closed.
|
|
|
|
|
|
- for (OAHashMap<int64_t, Point *>::Iterator it = p->neighbors.iter(); it.valid; it = p->neighbors.next_iter(it)) {
|
|
|
- Point *e = *(it.value); // The neighbor point.
|
|
|
+ for (const KeyValue<int64_t, Point *> &kv : p->neighbors) {
|
|
|
+ Point *e = kv.value; // The neighbor point.
|
|
|
|
|
|
if (!e->enabled || e->closed_pass == pass) {
|
|
|
continue;
|
|
@@ -390,13 +384,13 @@ real_t AStar3D::_estimate_cost(int64_t p_from_id, int64_t p_end_id) {
|
|
|
return scost;
|
|
|
}
|
|
|
|
|
|
- Point *from_point = nullptr;
|
|
|
- bool from_exists = points.lookup(p_from_id, from_point);
|
|
|
- ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ Point **from_entry = points.getptr(p_from_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!from_entry, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ Point *from_point = *from_entry;
|
|
|
|
|
|
- Point *end_point = nullptr;
|
|
|
- bool end_exists = points.lookup(p_end_id, end_point);
|
|
|
- ERR_FAIL_COND_V_MSG(!end_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_end_id));
|
|
|
+ Point **end_entry = points.getptr(p_end_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!end_entry, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_end_id));
|
|
|
+ Point *end_point = *end_entry;
|
|
|
|
|
|
return from_point->pos.distance_to(end_point->pos);
|
|
|
}
|
|
@@ -407,25 +401,25 @@ real_t AStar3D::_compute_cost(int64_t p_from_id, int64_t p_to_id) {
|
|
|
return scost;
|
|
|
}
|
|
|
|
|
|
- Point *from_point = nullptr;
|
|
|
- bool from_exists = points.lookup(p_from_id, from_point);
|
|
|
- ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ Point **from_entry = points.getptr(p_from_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!from_entry, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ Point *from_point = *from_entry;
|
|
|
|
|
|
- Point *to_point = nullptr;
|
|
|
- bool to_exists = points.lookup(p_to_id, to_point);
|
|
|
- ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_to_id));
|
|
|
+ Point **to_entry = points.getptr(p_to_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!to_entry, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_to_id));
|
|
|
+ Point *to_point = *to_entry;
|
|
|
|
|
|
return from_point->pos.distance_to(to_point->pos);
|
|
|
}
|
|
|
|
|
|
Vector<Vector3> AStar3D::get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
|
|
|
- Point *a = nullptr;
|
|
|
- bool from_exists = points.lookup(p_from_id, a);
|
|
|
- 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));
|
|
|
+ Point **a_entry = points.getptr(p_from_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!a_entry, Vector<Vector3>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ Point *a = *a_entry;
|
|
|
|
|
|
- Point *b = nullptr;
|
|
|
- bool to_exists = points.lookup(p_to_id, b);
|
|
|
- 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));
|
|
|
+ Point **b_entry = points.getptr(p_to_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!b_entry, Vector<Vector3>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_to_id));
|
|
|
+ Point *b = *b_entry;
|
|
|
|
|
|
if (a == b) {
|
|
|
Vector<Vector3> ret;
|
|
@@ -473,13 +467,13 @@ Vector<Vector3> AStar3D::get_point_path(int64_t p_from_id, int64_t p_to_id, bool
|
|
|
}
|
|
|
|
|
|
Vector<int64_t> AStar3D::get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
|
|
|
- Point *a = nullptr;
|
|
|
- bool from_exists = points.lookup(p_from_id, a);
|
|
|
- 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));
|
|
|
+ Point **a_entry = points.getptr(p_from_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!a_entry, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ Point *a = *a_entry;
|
|
|
|
|
|
- Point *b = nullptr;
|
|
|
- bool to_exists = points.lookup(p_to_id, b);
|
|
|
- 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));
|
|
|
+ Point **b_entry = points.getptr(p_to_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!b_entry, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id));
|
|
|
+ Point *b = *b_entry;
|
|
|
|
|
|
if (a == b) {
|
|
|
Vector<int64_t> ret;
|
|
@@ -527,17 +521,17 @@ Vector<int64_t> AStar3D::get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_
|
|
|
}
|
|
|
|
|
|
void AStar3D::set_point_disabled(int64_t p_id, bool p_disabled) {
|
|
|
- Point *p = nullptr;
|
|
|
- bool p_exists = points.lookup(p_id, p);
|
|
|
- ERR_FAIL_COND_MSG(!p_exists, vformat("Can't set if point is disabled. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point **p_entry = points.getptr(p_id);
|
|
|
+ ERR_FAIL_COND_MSG(!p_entry, vformat("Can't set if point is disabled. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point *p = *p_entry;
|
|
|
|
|
|
p->enabled = !p_disabled;
|
|
|
}
|
|
|
|
|
|
bool AStar3D::is_point_disabled(int64_t p_id) const {
|
|
|
- Point *p = nullptr;
|
|
|
- bool p_exists = points.lookup(p_id, p);
|
|
|
- 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));
|
|
|
+ Point *const *p_entry = points.getptr(p_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!p_entry, false, vformat("Can't get if point is disabled. Point with id: %d doesn't exist.", p_id));
|
|
|
+ Point *p = *p_entry;
|
|
|
|
|
|
return !p->enabled;
|
|
|
}
|
|
@@ -674,13 +668,13 @@ real_t AStar2D::_estimate_cost(int64_t p_from_id, int64_t p_end_id) {
|
|
|
return scost;
|
|
|
}
|
|
|
|
|
|
- AStar3D::Point *from_point = nullptr;
|
|
|
- bool from_exists = astar.points.lookup(p_from_id, from_point);
|
|
|
- ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ AStar3D::Point **from_entry = astar.points.getptr(p_from_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!from_entry, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ AStar3D::Point *from_point = *from_entry;
|
|
|
|
|
|
- AStar3D::Point *end_point = nullptr;
|
|
|
- bool to_exists = astar.points.lookup(p_end_id, end_point);
|
|
|
- ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_end_id));
|
|
|
+ AStar3D::Point **end_entry = astar.points.getptr(p_end_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!end_entry, 0, vformat("Can't estimate cost. Point with id: %d doesn't exist.", p_end_id));
|
|
|
+ AStar3D::Point *end_point = *end_entry;
|
|
|
|
|
|
return from_point->pos.distance_to(end_point->pos);
|
|
|
}
|
|
@@ -691,25 +685,25 @@ real_t AStar2D::_compute_cost(int64_t p_from_id, int64_t p_to_id) {
|
|
|
return scost;
|
|
|
}
|
|
|
|
|
|
- AStar3D::Point *from_point = nullptr;
|
|
|
- bool from_exists = astar.points.lookup(p_from_id, from_point);
|
|
|
- ERR_FAIL_COND_V_MSG(!from_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ AStar3D::Point **from_entry = astar.points.getptr(p_from_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!from_entry, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ AStar3D::Point *from_point = *from_entry;
|
|
|
|
|
|
- AStar3D::Point *to_point = nullptr;
|
|
|
- bool to_exists = astar.points.lookup(p_to_id, to_point);
|
|
|
- ERR_FAIL_COND_V_MSG(!to_exists, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_to_id));
|
|
|
+ AStar3D::Point **to_entry = astar.points.getptr(p_to_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!to_entry, 0, vformat("Can't compute cost. Point with id: %d doesn't exist.", p_to_id));
|
|
|
+ AStar3D::Point *to_point = *to_entry;
|
|
|
|
|
|
return from_point->pos.distance_to(to_point->pos);
|
|
|
}
|
|
|
|
|
|
Vector<Vector2> AStar2D::get_point_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
|
|
|
- AStar3D::Point *a = nullptr;
|
|
|
- bool from_exists = astar.points.lookup(p_from_id, a);
|
|
|
- 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));
|
|
|
+ AStar3D::Point **a_entry = astar.points.getptr(p_from_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!a_entry, Vector<Vector2>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ AStar3D::Point *a = *a_entry;
|
|
|
|
|
|
- AStar3D::Point *b = nullptr;
|
|
|
- bool to_exists = astar.points.lookup(p_to_id, b);
|
|
|
- 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));
|
|
|
+ AStar3D::Point **b_entry = astar.points.getptr(p_to_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!b_entry, Vector<Vector2>(), vformat("Can't get point path. Point with id: %d doesn't exist.", p_to_id));
|
|
|
+ AStar3D::Point *b = *b_entry;
|
|
|
|
|
|
if (a == b) {
|
|
|
Vector<Vector2> ret = { Vector2(a->pos.x, a->pos.y) };
|
|
@@ -756,13 +750,13 @@ Vector<Vector2> AStar2D::get_point_path(int64_t p_from_id, int64_t p_to_id, bool
|
|
|
}
|
|
|
|
|
|
Vector<int64_t> AStar2D::get_id_path(int64_t p_from_id, int64_t p_to_id, bool p_allow_partial_path) {
|
|
|
- AStar3D::Point *a = nullptr;
|
|
|
- bool from_exists = astar.points.lookup(p_from_id, a);
|
|
|
- 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));
|
|
|
+ AStar3D::Point **a_entry = astar.points.getptr(p_from_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!a_entry, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_from_id));
|
|
|
+ AStar3D::Point *a = *a_entry;
|
|
|
|
|
|
- AStar3D::Point *b = nullptr;
|
|
|
- bool to_exists = astar.points.lookup(p_to_id, b);
|
|
|
- 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));
|
|
|
+ AStar3D::Point **to_entry = astar.points.getptr(p_to_id);
|
|
|
+ ERR_FAIL_COND_V_MSG(!to_entry, Vector<int64_t>(), vformat("Can't get id path. Point with id: %d doesn't exist.", p_to_id));
|
|
|
+ AStar3D::Point *b = *to_entry;
|
|
|
|
|
|
if (a == b) {
|
|
|
Vector<int64_t> ret;
|
|
@@ -845,8 +839,8 @@ bool AStar2D::_solve(AStar3D::Point *begin_point, AStar3D::Point *end_point, boo
|
|
|
open_list.remove_at(open_list.size() - 1);
|
|
|
p->closed_pass = astar.pass; // Mark the point as closed.
|
|
|
|
|
|
- for (OAHashMap<int64_t, AStar3D::Point *>::Iterator it = p->neighbors.iter(); it.valid; it = p->neighbors.next_iter(it)) {
|
|
|
- AStar3D::Point *e = *(it.value); // The neighbor point.
|
|
|
+ for (KeyValue<int64_t, AStar3D::Point *> &kv : p->neighbors) {
|
|
|
+ AStar3D::Point *e = kv.value; // The neighbor point.
|
|
|
|
|
|
if (!e->enabled || e->closed_pass == astar.pass) {
|
|
|
continue;
|